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Abstract 


A computer program has been developed for the purpose of driving the NASA 
Langley Research Center Visual Motion Simulator (VMS). This program includes two 
new motion cueing algorithms, the optimal algorithm and the nonlinear algorithm. A 
general description of the program is given along with a description and flowcharts for 
each cueing algorithm, and also descriptions and flowcharts for subroutines used with the 
algorithms. Common block variable listings and a program listing are also provided. 

The new cueing algorithms have a nonlinear gain algorithm implemented that 
scales each aircraft degree-of-freedom input with a third-order polynomial. A description 
of the nonlinear gain algorithm is given along with past tuning experience and procedures 
for tuning the gain coefficient sets for each degree-of-freedom to produce the desired 
piloted performance. This algorithm tuning will be needed when the nonlinear motion 
cueing algorithm is implemented on a new motion system in the Cockpit Motion Facility 
(CMF) at the NASA Langley Research Center. 
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1. Introduction 


The flowchart for the new Langley Research Center Visual Motion Simulator 
(VMS) drive software is shown in Figure 2.1. Originally, three motion cueing algorithms 
were available: a modified version of the standard NASA adaptive algorithm 
(WCNTRL2), the optimal algorithm (WASHOPT3) and the nonlinear algorithm 
(NEWOPT4) discussed by Telban and Cardullo [1]. The modified adaptive algorithm is 
not operational. A one-time initialization of the cueing algorithm parameters and initial 
states is accomplished with a FORTRAN call to the subroutines WINIT2 and WINIT4. 
WINIT2 initializes variables exclusive to the adaptive and optimal algorithms, and 
variables common to both the optimal and nonlinear algorithms. WINIT4 initializes 
variables exclusive to the nonlinear algorithm, with symmetric matrices generated from 
initial vectors that contain the upper triangular elements. 

The real time program sends a flag to the motion cueing algorithm that indicates 
the current “mode” of the simulation, i.e. “RESET”, “HOLD”, or “OPERATE”. When 
the simulation is in the RESET mode, the motion base is driven slowly (in about 15 
seconds) to its neutral position. During the HOLD mode, the motion base is driven 
slowly from the neutral position to the required trim orientation. In the OPERATE mode, 
the motion cueing algorithm uses the aircraft states to drive the motion base in real time. 

The motion cueing algorithm produces the desired simulator displacement and 
attitude commands. These commands serve as input to the subroutine JACKDRVR, 
which performs a kinematic transformation to compute six actuator extensions and also 
activates a braking algorithm in the event any of the actuators reaches its extension limit. 
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The subroutine INVPLF performs an inverse kinematic transformation [2] to obtain the 
platform positions and attitudes from the computed leg extensions. 

The executive cueing algorithm subroutines WASHOPT3 and NEWOPT4 also 
incorporate special cueing effects. The augmented turbulence cue for the vertical mode 
developed by Telban and Cardullo [1] is implemented in each subroutine. In order to 
increase the sensation of the touchdown and ground effects, an approach proposed by 
Parrish and Martin [3] was implemented in each subroutine. This approach uses 
increased nonlinear gains for the vertical mode that are instantaneously activated upon 
touchdown, and runway roughness amplitude that is gradually added to the simulator z- 
axis displacement after touchdown. 

Table 1.1 provides a list of all the FORTRAN subroutines and common blocks 
used in the new software. The subroutines and common blocks used exclusively with the 
optimal algorithm are noted with an asterisk. All of the files have the extension .f unless 
otherwise noted. Listings of the variables for each common block used in the programs 
are given in Section 7. 


Table 1.1. FORTRAN Files for New Drive Software. 


gainopt3* 

liba 

ofil3* 

winit2* 

comint2.com 

gainopt4 

newopt4 

resetc2 

winit4 

matrixlc.com 

integ3* 

nfilr 

simq 

wtrim3 

nopt4.com 

integ4 

nfilx 

state4 


optint3.com* 

invplf 

nfily 

vmult 


wopt3.com 

jackdrvr 

nfilz 

washopt3* 
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Table 1.2 provides a list of the aircraft model inputs required for the motion 


cueing algorithms. Given are the cueing algorithm FORTRAN variables. 


Table 1.2. Motion Cueing Algorithm Inputs. 



Optimal 

(IWASH=2) 

Nonlinear 

(IWASH=3) 

Aircraft Accel, at Motion Base Centroid 

ACA(3) 

ACA(3) 

Aircraft Angular Velocity 

WAA(3) 

WAA(3) 

Aircraft Euler Angles 

BETAA(3) 

BETAA(3) 

Time Step Size 

DT 

DT 

In-Air/On-Ground Flag 

SQWASHI 

SQWASHI 

Aircraft Ground Speed 

VGSPD 

VGSPD 

Vertical Gust 

WGUST 

WGUST 
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Figure 1.1. Langley Visual Motion Simulator (VMS) Drive Software Flow Chart. 
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2. Background Information 

2.1. NASA Langley Visual Motion Simulator (VMS) 

The NASA Langley Visual Motion Simulator (VMS), shown in Figure 2.1, is a 
general-purpose flight simulator consisting of a two-crewmember cockpit mounted on a 
60-inch stroke six-degree-of- freedom synergistic motion base [4], [5]. 



Figure 2.1. NASA Langley Visual Motion Simulator (VMS). NASA Langley 
Research Center, Hampton, Virginia. 

Motion cues are provided in the simulator by the extension or retraction of the six 
hydraulic actuators of the motion base relative to the simulator neutral position. The 
NASA adaptive algorithm and the new optimal and nonlinear algorithms were used to 
drive the motion base during the tuning of the new algorithms and the piloted test 
evaluation. 

The cockpit of the VMS, shown in Figure 2.2, is designed to accommodate a 
generic transport aircraft configuration on the left side and a generic fighter or rotorcraft 
configuration on the right side. Both sides of the cockpit are outfitted with three heads- 
down CRT displays (primary flight display, navigation/map display, and engine display), 
a number of small standard electromechanical circular instruments and a landing gear 
handle mounted in the instrument panel. The left side contains a two-axis side stick 
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control loader, and the right side contains a control loaded two-axis center stick. Both 
sides contain control loaded rudder systems. The center aisle stand is outfitted with a 
control display unit, a four-lever throttle quadrant, a flap handle, a speed brake handle, 
and a slats handle. The cockpit is outfitted with four collimated window display systems 
to provide an out-the-window visual scene. During the piloted evaluations, the test 
subject flew from the left seat of the cockpit, while an observer/test conductor rode in the 
right seat. 



Figure 2.2. Visual Motion Simulator Cockpit. NASA Langley Research Center, 
Hampton, Virginia. 

The simulator includes a high fidelity, highly nonlinear mathematical model of a 
Boeing 757-200 aircraft, complete with landing gear dynamics, gust and wind models, 
flight management systems, and flight control computer systems. For this study, the test 
subjects flew the simulated aircraft in the manual control mode (without the autopilot), 
and with manual throttle control (without the auto throttle). 

The out-the window visual scene is driven by an Evans and Sutherland ESIG 
3000/GT computer generated image system. The visual database represented the 
Dallas/Fort Worth airport and its surrounding terrain. The study utilized runways 18L 
and 18R for approach maneuvers and runway 18R for takeoff maneuvers. The runways 
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were equipped with approach lights, precision approach path indicator lights, runway 
markings, and signage. The database included all runways and taxiways, and all airport 
structures and buildings. All tests were conducted in a daylight environment with full 
visibility. 

2.2. Reference Frames 

A series of reference frames are used in the definition of the motion cueing 
algorithms. These reference frames are defined below and are shown in Figure 2.3. 

2.2.1. Aircraft Center of Gravity 

The aircraft center of gravity reference frame Fr CG has its origin at the center of 
gravity of the aircraft. Frame Fr CG has an orientation for X CG , Y CG , and Z CG that is parallel 
to reference frames Fr s and Fr A . 

2.2.2. Simulator 

The simulator reference frame Fr s has its origin at the centroid of the simulator 
payload platform, i.e. the centroid of the upper bearing attachment points. The origin is 
fixed with respect to the simulator payload platform. X s points forward and Z s points 
downward with respect to the simulator cockpit, and Y s points toward the pilot's right 
hand side. The x-y plane is parallel to the floor of the cockpit. 

2.2.3. Aircraft 

The aircraft reference frame Fr A has its origin at the same relative cockpit location 
as the simulator reference frame Fr s . FrA has the same orientation for X A , Y A , and Z A 
with respect to the cockpit as the simulator frame Fr s . 
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2.2.4. Inertial 


The inertial reference frame Fri is earth-fixed with Z r aligned with the gravity 
vector g. Its origin is located at the center of the fixed platform motion base. Xi points 
forward and Yi points to the right hand side with respect to the simulator pilot. 

2.2.5. Reference Frame Locations 

In Figure 2.3 are four vectors that define the relative location of the reference frames. Ri 
defines the location of Fr s with respect to Fr,. R s defines the location of Frps with respect 
to Fr s . Similarly, R A defines the location of Fr PA with respect to Fr A where R A = R s . R C g 
defines the location of Fr A with respect to Fr CG . 



Figure 2.3. Reference Frame Locations. Adapted from Wu [6]. 
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2.3. Coordinate Transformations 

The orientation between the body-fixed simulator reference frame Fr s and the 
inertial reference frame Fr, can be specified by three Euler angles: p = [(/) 9 !//]' that 
define a sequence of rotations that carry Fr s into Fiy A vector V expressed in the two 
frames can be related by the transformation matrix L IS (Fp to Fr s ) or L SI (Fr s to Fr,), with 
V s = L IS V‘ and V 1 = L SI V S , where L IS = , and 

cos^cos^ sin^sin^cos^-cos^sin^ cos^sin^cos^ + sin^sin^ 

L SI = cos^sin^r sin 0 sin <9 sin ^-l- cos 0 cos cos 0 sin <9 sin sin 0 cos ^ . (2.1) 

-sin <9 sin^cos# cos^cos# 

The angular velocity of Fr s with respect to Fr, can be related to the Euler angle 
rates p by the following expression. Let co A represent the components of this angular 
velocity in frame Fr s , then p = T s co A , where 

1 sin 0 tan 9 cos 0 tan 9 

T s = 0 cos^ -sin^ . (2.2) 

0 sin 0 sec <9 cos 0 sec <9 

Note that in this example, the body-fixed aircraft reference frame Fr A can replace 
the body-fixed simulator reference frame Fr s . 

The subroutine LIBA computes the coordinate transformations of Eqs. (2.1) and 
(2.2) as shown in the flowchart given in Figure 2.4. 
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ACA, WAA, BET A A 


SINPHI, SINTH, SINPSI 
COSPHI, COSTH, COSPSI 


LIA, TA 


A2, BET A AD 


Figure 2.4. LIBA Subroutine Flowchart. 

2.4. Nonlinear Input Scaling 

Limiting and scaling are applied to both aircraft translational input signals a^ and 
rotational input signals co^ . Limiting and scaling modify the amplitude of the input 
uniformly across all frequencies. Limiting is a nonlinear process that clips the signal so 
that it is limited to be less than a given magnitude. Limiting and scaling can be used to 
reduce the motion response of a flight simulator. A third-order polynomial scaling was 
developed [6] and has been implemented in the new simulator motion cueing algorithms. 

When the magnitude of the input to the simulator motion system is small, the gain 
is desired to be relatively high, or the output will be below the pilot’s perception 
threshold. When the magnitude of input is high, the gain is desired to be relatively low or 
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the simulator may attempt to go beyond the hardware limits. Let us define the input as x 


and the output as y. Now define x in , is as the expected maximum input and y max as the 
maximum output, and s 0 and s 1 as the slopes at a = 0 and x = A max respectively. Four 
desired characteristics for the nonlinear scaling are expressed as: 

(1) x = 0 => y = 0, 

(2) ^ ~ ^max y ~ y max ’ 



A third-order polynomial is then employed to provide functions with all the 
desired characteristics. This polynomial will be of the form 

y = c 3 x 3 + c 2 x 2 + CjX + c 0 (2.3) 

where 

c 0 =0, 
c \ = V 

C 2 = Tnax (^max “ 2 ^ () ^max “ 5 Amax ). 

C 3 = ^max (Vmax “ 2y max + ). 

One example of this polynomial gain is shown in Figure 2.5, with parameters set as 

Anax= 10 > y m ax = 6 > S 0 = 1-0, = 0.1. 

The subroutine GAINOPT3 is used to compute the polynomial scaling 
coefficients for each aircraft degree-of-freedom input for the optimal algorithm. The 
flowchart for GAINOPT3 is given in Figure 2.6. The subroutine GAINOPT4 computes 
the coefficients for the nonlinear algorithm, and follows the same variable flow as shown 
in Figure 2.6. Each subroutine contains scaling coefficients specific to either the optimal 
algorithm (GAINOPT3) or the nonlinear algorithm (GAINOPT4). 


11 



Nonlinear Scaling 



Figure 2.5. Nonlinear Input Scaling. 



A2, BETAAD 


AA4, BA4 


AA4, BA4 


A2N, BADN 


Figure 2.6. GAIN OPT3/ GAIN OPT4 Subroutine Flowchart. 
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2.5. Actuator Geometry 

The geometry of a six-degree-of-freedom synergistic motion system is given in 
Figure 2.7. The relevant vectors relating the locations of the upper and lower bearings of 
the j-th actuator are shown below in Figure 2.8. 



Figure 2.7. Geometry of a Six-Degree-of-Freedom Motion System. Adapted from 
Wu (1997). 
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Figure 2.8. Vectors for the j-th Actuator. 


In Figure 2.8, O s and Oi are the centroids of the motion platform and fixed 
platform respectively, and are also respectively the origins for Frs and Fri. It can be seen 
that the relation among those vectors is 

R I+ A;= Rj =B; + Z r (2.4) 

The actuator length vector can then be found from 

h= A )+ R i~K ( 2 - 5 ) 


The expression of /. in the inertial reference frame Fri is desired: 


^Ai +Rl -Bj 
= L ls A j s + Rj -Bj, 


(2.6) 


where A* are the coordinates of the upper bearing attachment point of the j-th actuator in 
Frs and B‘ are the coordinates of the lower bearing attachment point of the j-th actuator 
in Fri. The actuator extension is computed from a neutral platform position, where 
Rj = 0 , therefore 

A/j = L IS A? + ARj. (2.7) 
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The subroutine JACKDRVR is used to compute the actuator extension lengths 
that are derived from the motion platform displacements and attitudes in Eq. (2.7). The 
flowchart for JACKDRVR is given in Figure 2.9. The variable inputs and outputs for the 
subroutine INVPLF are also shown in Figure 2.9. The actuator extension limiting 
performed within JACKDRVR is discussed in Section 6 and Appendix A. 



Figure 2.9. JACKDRVR/INVPLF Subroutine Flowchart. 
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2.6. Augmented Turbulence Cue 

Reid and Robinson [7] first addressed the problem of producing acceptable 
motion cues to turbulence inputs. They noted that heave is the most critical cue in 
representing turbulence, but is also the most restricted cue when constraining motion 
within the platform geometry. To overcome this limitation, they developed an approach 
in which a second set of aircraft equations of motion driven only by the turbulence inputs 
is employed. The output from this augmented channel is then added to the output from 
the primary equations of motion, being driven by both turbulence and the pilot control 
inputs, before serving as input to the motion system. A similar approach to that 
developed by Reid and Robinson [7] has been implemented and is shown in Figure 2.10. 
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Figure 2.10. Optimal Algorithm Vertical Mode with Augmented Turbulence 
Channel. 


The input to the augmented channel is the z-axis component w G of the turbulence 
vector G. Reid and Robinson showed that w G is the dominant turbulence component 
needed in producing vertical acceleration due to turbulence. The secondary flight 
equations can then be represented by a transfer function H a (s). The secondary 
acceleration a', is then scaled with a constant gain K G . Both the primary and secondary 
signals are then combined before input to the vertical motion cueing filter W 22 - 
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From a simulated Being 757-200 aircraft test run, a system identification of 
aircraft vertical accelerations in response to turbulence was performed. The transfer 
function H G (s) was then created not only to represent the acceleration, but also 
incorporate some desired motion cueing characteristics, i.e., attenuated low-frequency 
content and increased high-frequency content. The following second-order transfer 
function was obtained for H a (s ): 


H a {s) = 0.1 


(2.4s + l)(2.4s + l) 
(0.4.v + 1 ) ( 0 . 1 .s + 1)' 


(2.8) 


For the optimal algorithm, a gain of K a equal to 0.8 was chosen to maximize the 
desired sensation of turbulence while sustaining the actuator extensions within the motion 
limits. A similar implementation to that shown in Figure 2.10 was applied for the 
nonlinear algorithm. In this approach, the linear cueing filter W 22 was replaced with the 
nonlinear heave filter, with the gain K c set equal to 1.2. 

The transfer function given in Eq. (2.8) was implemented in state space form for 
the optimal and nonlinear algorithms in the subroutines WASHOPT3 and NEWOPT4 
respectively. The transfer function coefficients and gain terms for each algorithm are 
initialized in the subroutines WINIT2 and WINIT4 respectively. 

2.7. Trim Algorithm 

Martin [8] reported that before going from the RESET mode to the OPERATE 
mode, several seconds are needed in the HOLD mode to produce pitch and roll angles 
that balance out any steady state longitudinal or lateral accelerations that may occur at 
time zero. In the subroutine WTRIM3 shown in Figure 2.11, a first-order washout filter 
is used to allow a smooth transition from the platform neutral state to the desired pitch 
angle: 
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(2.9) 


&(s) _ b x s 
a x {s) s + a 0 ' 

Both b i and a„ are chosen to produce the desired transition rate and attitude. As noted in 
Figure 2.11, Eq. (2.9) is integrated to produce the pitch attitude. Similar computations 
are performed to obtain the roll velocity (j ) , which is then integrated to produce the 
desired roll angle. 



Figure 2.11. WTRIM3 Subroutine Flowchart. 
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3. Optimal Motion Cueing Algorithm 

3.1. Algorithm Description 

The theory and development of the optimal algorithm is well discussed by Telban 
and Cardullo [1], The problem is to determine a transfer function matrix W(s) that 
relates the desired simulator motion input to the aircraft input such that a cost function 
constraining the pilot sensation error (between simulator and aircraft) is minimized. A 
mathematical model of the human vestibular system [1] is used in the filter development. 
The optimal algorithm uses an off-line program [1] to generate the desired transfer 
functions W(s) which are then implemented on-line. W(s) will relate the simulator 
commands to the aircraft states by u s = W(s) x u A . The block diagram for the on-line 
algorithm implementation is shown in Figure 3.1. Similar to the NASA adaptive 
algorithm [8], there are separate filtering channels for the translational and rotational 
degrees of freedom with the cross-feed path providing the tilt coordination cues. 


Pa 



Figure 3.1. Optimal Algorithm Implementation. 
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The aircraft acceleration input vector is first transformed from the simulator body 
frame Fr s to the inertial frame Fr, as shown in Eq. (2.1). Nonlinear scaling in 
combination with limiting as described in Section 2.4 is then applied to the aircraft 
inputs. The scaled inertial acceleration a A is then filtered through the translational filter 
W 22 to produce a simulator translational acceleration command S, . This acceleration is 
integrated twice to produce the simulator translational position command Sj . 

The aircraft angular velocity input io A is transformed to the Euler angular rate 
vector P A as shown in Eq. (2.2), and is limited and scaled similar to the translational 
channel. This input is then passed through the rotational filter W u to produce the vector 
P SR . The tilt coordination rate P ST is formed from the acceleration a A being passed 

through the tilt coordination filter W n . The summation of P ST and P SR yields P s , which 
is then integrated to generate P s , the simulator angular position command. 

The simulator translational position S 1 and the angular position P s are then used 

to transform the simulator motion from degree-of-freedom space to actuator space as 
given in Eq. (2.7), generating the actuator commands required to achieve the desired 
platform motion. 

The motion cueing filter W(s) is represented by a Laplace transform transfer 
function: 

_ b n s" + b„_ x s n ~' + --- + b 1 s + b 0 (3 ^ 

S " + £l n _jS" ' + • — h Cl^S + Uq 

For numerical computation, Eq. (3.1) is realized in state space by the observable 
canonical form [9]: 


W(s) 
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x(t) = A x(t) + Bu(t), 


(3.2) 


where the matrices A and B are 


—a , 

n — 1 

1 

0 • 

• 0“ 
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—a ~ 

n— 2 
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1 • 

• 0 


K - 2 - 

-«„-2 K 




• 0 

, B = 



-a x 
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0 • 

• 1 


V 

~ a A 

_ ~ a o 
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0 • 

• °_ 


V 

1 

0 
<3 

1 


andy(t) = [l 0 0 ••• 0]x(t) + b n \x(t) . 

The state equations of Eq. (3.2) are integrated with a 2 nd -order Runge-Kutta 
method [10]. This ensures stable responses for sample rates of at least 32 Hz. 

3.2. Program Implementation 

The flowchart for the optimal algorithm subroutine WASHOPT3 is shown in 
Figure 3.2. The subroutine RESETC2 is similar to that described by Martin [8], 
providing a smooth return of the motion base to the neutral position in the reset mode. 
The subroutine WTRIM3 uses a set of first-order filters as shown in Section 2.7 to 
produce a smooth buildup of the motion platform pitch and roll tilt angles to the trim 
position based upon the aircraft trim states. The subroutine LIBA transforms the aircraft 
translational accelerations from the body to the inertial reference frame, and the aircraft 
angular velocities from the body to the Euler reference frame as discussed in Section 2.3 
and shown in Figure 2.4. These transformed aircraft states then undergo a nonlinear 
scaling with the subroutine GAINOPT3 described in Section 2.4 and shown in Figure 2.6. 
The subroutine OFIL3 computes the responses to the linear filters for each mode. The 
subroutine INTEG4 then computes the desired simulator displacements and attitudes, 
taking the tilt coordination into account. 
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The flowchart for OFIL3 is given in Figure 3.3. Each filter is formulated in state 
space as shown in Eq. (3.2), and is integrated using the 2 nd -order Runge-Kutta method. 
The outputs of OFIL3 are the desired motion cues that drive the simulator, i.e., three 
translational accelerations, three rotational angular velocities, and two tilt angular 
velocities that represent the tilt coordination cues. 

The flowchart for INTEG3 is given in Figure 3.4. The simulator translational 
accelerations and angular velocities are first integrated to obtain the translational 
displacements and angular positions. The commanded pitch tilt position 6 m . is 
determined from both the “desired” tilt position 0 ST and the difference between the 
desired and commanded tilt positions: 

=O.OO5(0» l -0£') + (0«> -<*>), (3.3) 

Where the superscripts (i) and (i-1) refer to the current and prior time steps. The 
commanded tilt position 0 STL is then computed, taking into account the tilt velocity limit 
0 hm = 5 deg/ sec and the time step At: 

d STL = 0 STL + maX ^ ^ STL )]• ( 3 - 4 ) 

The difference between the desired and commanded tilt angles is then used to generate 
additional translational response and achieve coordination between the translational and 
tilt channels: 

X com = X des ~ ^Sz ( ^ST ~ @ STL ) ’ ( 3 -5) 

where R s , is the radius from the motion platform centroid to the pilot’s head. Similar 
computations are performed to obtain the commanded roll tilt position (f)^ L and the 
commanded displacement y com . 
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Figure 3.2. Optimal Algorithm Flowchart. 
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Figure 3.3. OFIL3 Subroutine Flowchart. 


24 













PHID, THED, PSID 

Figure 3.4. INTEG3 Subroutine Flowchart. 
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4. Nonlinear Motion Cueing Algorithm 

4.1. Algorithm Description 

The theory and development of the nonlinear algorithm is well discussed by 
Telban and Cardullo [1], The algorithm is formulated as a linear optimal control problem 
similar to the optimal algorithm, but is also updated in real time with a nonlinear control 
law. Furthermore, it incorporates models of the human vestibular sensation system along 
with an integrated visual- vestibular perception model [1], The block diagram for the on- 
line implementation is shown in Figure 4.1. Similar to the optimal algorithm, there are 
separate filtering channels for the translational and rotational degrees of freedom with the 
cross-feed path providing the tilt coordination cues. Telban and Cardullo [1] reported 
that for the pitch and roll rotational channels, no benefit resulted from updating the 
Riccati equation in real time; thus the nonlinear filters are replaced with unity-gain filters. 



Figure 4.1. Nonlinear Algorithm Implementation with Unity-Gain Pitch Filter. 

A nonlinear control law is implemented to generate a scalar coefficient crthat is a 
function of the simulator motion system states: 
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(4.1) 


cc — x A Q 2 x d , 

where Q 2 is a weighting matrix that is at least positive semi-definite. As the computed 
system states increase in magnitude, i.e., with large commanded platform displacements 
and velocities, then a increases, resulting in faster control action to quickly wash out the 
platform to its neutral state. For small commands there will be limited control action, 
resulting in motion cues sustained for longer durations. 

The solution of the algebraic Riccati equation is given as [1] 

(A' + al) T P(a) + P(a)(A / + al) - P(cr)BR 2 1 B T P(cr) + R[ = 0, (4.2) 

where A' and B are system matrices and Rj and R 2 are the standard optimal control 
weighting matrices defined by Telban and Cardullo [1] in the algorithm development. 
The system matrix A' is augmented with a times the identity matrix I. The solution of 
Eq. (4.2) from the linear optimal algorithm that was computed off-line in MATLAB™ is 
used as the initial solution for the first time step. The Riccati equation of Eq. (4.2) is then 
updated in real time with a structured neural network developed by Ham and Collins [11] 
that is shown in Figure 4.2. 
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Figure 4.2. Structured Neural Network for Solving the Riccati Equation. 

The error signal v ( t ) in Figure 4.2 is given as 

v(r)=[p(»)sp(r)- a; t p(,)-p(,)a; -r;]z( ( ), (4.3) 

where A^ = A' + al and S = B 1 R )' B . The external excitatory vector input signals z(/) 
are a set of linearly independent bi-polar vectors given as 

z (1, =[l -1 ••• -1], z (2) = [-1 1 -l],z ( '°=[-l -1 ••• 1], (4.4) 

where each vector z a) is presented once to the neural network in an iteration, i.e., for one 
iteration there are a total of n presentations of the training step given in Eq. (4.4), with the 
solution P {k) updated with each training step. The update term AP(k) is given as 

AP(k)=[A;(k)v(k)z(k) + v(k)z T (k)A' a (k)-v(k)p T (k)S], (4.5) 

where p(t) = P(t)z(t) as shown in Figure 4.2. The updated Riccati equation solution 
P [k + 1) is then computed: 

P(k + 1) = P(k) + //AP(k), (4.6) 

where // > 0 is the learning rate parameter. 
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The feedback matrix K (a) is partitioned corresponding to the partition of the 


state vector x [1]: 

u s =-[K,(«) K 2 («)] X - -K,(«)u 4 , (4.7) 

L X d _ 

and the Riccati equation solution P(et) can be partitioned as 

p n {a) P 12 {a) P \ 3 {a) 

P {a)= P 21 (a) P 22 (a) P 2} (a) , (4.8) 

_ P 3l(«) P 23(«) P 33(«)_ 

where the partitions correspond to the partitions of the system matrix A. 
The partitioned feedback matrix is then given as 

K, (a) = R; 1 [B';p„ + B d p 21 + DjQC v ] 

K ! («) = R 2 , [ B '; p 1! + B J ' p !i ] (4.9) 

K, (a) = R; 1 [b; p „ + B d rp 2J - DjQC v ], 

where the matrices B v , B d , and D V QC V are defined by Telban and Cardullo [1], and by 
symmetry, P 12 = P, 1 , . The resulting state equations are then computed in real time: 
TIK-W) -BvKj(ff) Tx.l -B T (I + K 3 (ar)) 

_i d J L - B d K ,(«) A d -B a K 2 (a)}x d J [ -B d K 3 (o') J"*’ 

with the matrices A v and A d defined by Telban and Cardullo [1], These state equations 
are then integrated with a 2 nd -order Runge-Kutta method [10]. 

4.2. Program Implementation 

The nonlinear algorithm was developed to achieve the desired motion cues at an 
update rate of 60 Hz. Since the computer image generator, which provides the out-the- 
window visual imagery to the simulator pilot, also runs at 60 Hz, the motion cues would 


30 



be synchronous with the visual cues. However, because of the computer operating 
system, the real time operating system and the I/O system on the Langley real time 
computing system, the minimum interval must be an integer multiple of 125 |isec and a 
power of 2. Therefore, a time step of 16 msec or an update rate of 62.5 Hz was selected 
for the real time implementation and the pilot tests. 

The flowchart for the nonlinear algorithm subroutine NEWOPT4 is shown in 
Figure 4.3. The RESET/HOLD modes and transformation subroutines are identical to the 
optimal algorithm discussed in the preceding section. The transformed aircraft states 
then undergo a nonlinear scaling with the subroutine GAINOPT4. 

The subroutine NFILX accomplishes the task of solving the Riccati equation in 
real time for the longitudinal mode. Similarly, NFILY, NFILZ, and NFILR compute the 
Riccati equation solutions for the lateral, vertical, and yaw mode respectively. The 
feedback matrices and updated motion states for each mode are then computed in the 
subroutine STATE4. The subroutine INTEG4 computes the desired simulator 
displacements and attitudes, taking into account the tilt coordination limits. Note that 
STATE4 is computed four times when in the HOLD mode; this was done to produce 
faster convergence to the neutral state while eliminating discontinuities when 
transitioning from the HOLD to the OPERATE mode. 

The flowchart for NFILX is given in Figure 4.4. The coefficient a is computed 
from the simulator motion states given in Eq. (4.1). For each training step, the 
corresponding input vector z given in Eq. (4.4) is referenced. The update term AP given 
in Eq. (4.5) and its matrix transpose AP 1 are then computed and used to update the 
Riccati equation solution P(k + l) given in Eq. (4.6). The flowcharts for NFILY, 
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NFILZ, and NFILR are given in Figure 4.5, Figure 4.6, and Figure 4.7 respectively. The 
variable flow for each subroutine is identical to that shown for NFILX in Figure 4.4. 
Note that each subroutine uses the motion states specific to its mode for computing the 
coefficient a , with the system variables defined for each mode ending with the same 
letter as the subroutine name. 

During implementation on the NASA Langley real time system, it was discovered 
that for a time step of 16 msec, the real time requirement for the nonlinear algorithm was 
not being met. The baseline software for the nonlinear algorithm, with all presentations 
of the excitatory vector z (t) given for each mode given in Eq. (4.4), i.e., n presentations 

for an « th -order system, resulted in an average CPU time of 34 msec. This CPU time also 
includes the contributions of the aircraft model and control loader. In the subroutines 
NFILX, NFILY, NFILZ, and NLILR, matrix operations were optimized by taking 
advantage of the matrices A' and S being sparse, i.e., eliminating computations involving 
matrix elements of zero. In addition, the symmetry of the Riccati solution P and other 
matrices computed in the subroutines were also exploited to reduce computation. 
Optimizing matrix operations resulted in an average CPU time of 24 msec. Reducing the 
number of presentations of the vector z (t) for each mode to the first three vectors 

produced a CPU time of 12 msec, which meets the real time requirement. Telban and 
Cardullo [1] reported that this change resulted in imperceptible degradation of the motion 
cues. 

The flowchart for STATE4 is given in Ligure 4.8. The variable flow for 
computing the longitudinal mode simulator states is shown in detail. The partitions of the 
feedback matrix are computed as given in Eq. (4.9). The state equations are then updated 
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as given in Eq. (4.10) and integrated with a 2 nd -order Runge-Kutta method, resulting in 
the updated simulator motion states. The computation of the lateral, vertical, and yaw 
states follows the same variable flow as the longitudinal states, with the system variable 
names corresponding to the respective mode. 

The flowchart for INTEG4 is given in Figure 4.9. The simulator translational 
accelerations, rotational angular velocities, and tilt angular velocities are computed as 
given in Eq. (4.7). The remaining computations are identical to those performed in the 
subroutine INTEG3 for the optimal algorithm as shown in Figure 3.4, and discussed in 
Section 3.2. 
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Figure 4.3. Nonlinear Algorithm Flowchart. 
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Figure 4.4. NFILX Subroutine Flowchart. 
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Figure 4.5. NFILY Subroutine Flowchart. 
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Figure 4.6. NFILZ Subroutine Flowchart. 
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Figure 4.7. NFILR Subroutine Flowchart. 
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Figure 4.8. STATE4 Subroutine Flowchart. 
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Figure 4.9. INTEG4 Subroutine Flowchart. 
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5. Nonlinear Algorithm Tuning 

This section covers the procedures needed for tuning the nonlinear algorithm in 
order to produce the desired pilot performance. It is expected that the algorithm will 
require tuning with either a change of aircraft dynamics (the initial pilot tuning was done 
with the Boeing 757-200 aircraft model) or implementation on a new motion system such 
as the Cockpit Motion Facility (CMF). 

The “off-line” development of the nonlinear algorithm mentioned in Section 4.1 
and discussed in greater detail by Telban and Cardullo [1], resulted in a set of filters 
consisting of system matrices and an initial Riccati equation solution implemented for 
each mode. Each filter was optimized with single degree-of-freedom responses for 
synergistic six-degree-of-freedom motion systems similar to Figure 2.7. Except for 
special circumstances driven by suggested future research [1], these filters do not need to 
be re- synthesized. Only the nonlinear scaling coefficients for each degree-of-freedom, 
given in Eq. (2.3), will need to be adjusted to accommodate changes in aircraft dynamics 
or motion platform geometry. 

It is expected that the optimal algorithm discussed in Section 3 will be used 
infrequently. However, in the event that future work with the optimal algorithm is 
desired, the same procedures described in this section for tuning the nonlinear algorithm 
gains can be applied. 

5.1. Prior Tuning Experience 

In order to determine the nonlinear scaling (gain) coefficients for each degree-of- 
freedom that resulted in the most desired pilot performance, a trained simulator pilot 
executed a series of pilot controlled maneuvers with the optimal algorithm on the Visual 
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Motion Simulator (VMS). A series of maneuvers were first executed with the 

coefficients determined prior to testing. Coefficients for each degree-of-freedom were 

then adjusted until the simulator pilot subjectively felt the desired perception and 

performance were reached, while ensuring that the simulator motion platform limits were 

not exceeded. The following maneuvers were executed for both algorithms: 

Straight Approach and Landing (with varying wind from head to tail) 
Offset Approach and Landing (with and without turbulence) 

Pitch, Roll, and Yaw Doublets 
Throttle Increase and Decrease 
Coordinated Turn 

Ground Maneuvers (taxiing, effect of aircraft brakes) 

Takeoff from Full Stop. 

The optimal algorithm resulted in motion cues with which the simulator pilot 
commented he had more control and confidence in comparison to the NASA adaptive 
algorithm. For both pitch and roll doublets, a fast response was observed when changing 
directions. On takeoffs, the optimal algorithm was found to be easier to pitch up to the 
desired attitude and control the aircraft. A noticeably large side force was observed with 
the coordinated turn maneuver. By reducing the gains for the roll degree-of-freedom, this 
side force was reduced to a minimal sensation. The pitch gains were decreased to reduce 
the likelihood of entering the braking region or exceeding the actuator limits. Reducing 
the gains for both roll and pitch degrees-of- freedom still yielded acceptable motion cues. 

The severe turbulence effects that were included with the offset approach and 
landing maneuver were hardly noticeable. An increase of the vertical gain coefficients 
resulted in increased cues, but still less than satisfactory. This increase in the vertical 
gains (coupled with an increase of the surge gains) resulted in forward surge cues that are 
more coordinated with the pitch cues, and a larger aft surge cue (initially, the aft cue was 
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noticeably smaller than the forward cue). The unsatisfactory turbulence cues resulted in 
the inclusion of the augmented motion cue driven by the vertical gust. 

A second pilot tuning evaluation was later performed on the VMS with both the 

optimal and nonlinear algorithms, with augmented turbulence cues implemented for both 

algorithms. A series of maneuvers were first executed with the polynomial gain 

coefficients determined prior to testing. Coefficients for each degree-of-freedom were 

then adjusted until the simulator pilot subjectively felt the desired perception and 

performance were reached, while ensuring that the simulator motion platform limits were 

not exceeded. The following maneuvers were executed for both algorithms: 

Straight Approach and Landing (with varying wind from head to tail) 
Offset Approach and Landing (with and without turbulence) 

Takeoff from Full Stop (with and without engine failure) 

Ground Maneuvers (taxiing, effect of aircraft brakes). 

No additional tuning was needed for either the straight-in or offset approach 
maneuvers. However, both algorithms showed a tendency to exceed the actuator limits 
of the motion system with the takeoff maneuver. Reducing the surge gains for the 
optimal algorithm and both the surge and pitch gains for the nonlinear algorithm resulted 
in platform motion within the actuator limits during the takeoff maneuvers. The 
augmented turbulence gain terms for the optimal and nonlinear algorithms discussed in 
Section 2.6 were adjusted to produce the desired turbulence cues. 

Table 5.1 lists the resulting nonlinear gains by degree-of-freedom implemented 
for each algorithm. From Eq. (2.3), the coefficients c u c 2 , and c 3 are given for each 
degree-of-freedom. 
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Table 5.1. Nonlinear Gain Coefficients for the Cueing Algorithms. 



Optimal Algorithm 

Nonlinear Algorithm 

Degree-of- 

Freedom 

Cl 

C2 

C3 

Cl 

C2 

C3 

Surge (X) 

0.6 

-0.055 

0.002 

0.5 

-0.05 

0.002 

Sway (Y) 

0.5 

-0.055 

0.002 

0.4 

-0.035 

0.001 

In- Air (Z) 

0.6 

-0.082 

0.0038 

0.6 

-0.082 

0.0038 

On-Ground (Z) 

1.3 

-0.0375 

0.0003 

2.0 

-0.05 

0.0 

Roll (p) 

0.3 

-0.3 

0.1 

0.3 

-0.3 

0.1 

Pitch (q) 

0.4 

-0.54 

0.26 

0.3 

-0.3 

0.1 

Yaw (r) 

1.1 

-1.46 

0.64 

1.1 

-1.46 

0.64 


5.2. Tuning Procedure 

Nonlinear gain coefficients can be computed from Eq. (2.3) for each degree-of- 
freedom with the use of an Excel™ spreadsheet. Using this approach, a set of 
coefficients for each degree-of-freedom can be tested with piloted simulations, resulting 
in the desired nonlinear gains for the cueing algorithm. Table 5.2 provides five sets of 
coefficients computed for each in-air degree-of-freedom (excluding yaw) for the 
nonlinear algorithm, with an additional set of coefficients for the on-ground heave gain. 
The yaw coefficients are omitted since these gains remained unchanged with initial 
tuning of both algorithms. 

Each degree-of-freedom consists of five sets of gain coefficients, which are 
labeled to identify both the degree-of-freedom and the coefficient set, e.g., the roll 
degree-of-freedom coefficient sets are labeled from NP1 to NP5. The nonlinear gain 
coefficients for the nonlinear algorithm are initialized in data statements in the subroutine 
WINIT4. These data statements can be edited as individual degree-of-freedom 
coefficient sets are updated during the tuning process. 
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Table 5.2. Nonlinear Gain Coefficient Pilot Test Spreadsheet, 


ROLL 



SO 

SI 

XMAX 

YMAX 

GP4(1) 

GP4(2) 

GP4(3) 

NP1 

0.26 

0 

1 

0.1 

0.26 

- 0.22 

0.06 

NP2 

0.28 

0 

1 

0.1 

0.28 

- 0.26 

0.08 

NP3 

0.3 

0 

1 

0.1 

0.3 

- 0.3 

0.1 

NP4 

0.32 

0 

1 

0.1 

0.32 

- 0.34 

0.12 

NP5 

0.34 

0 

1 

0.1 

0.34 

- 0.38 

0.14 


PITCH 



SO 

SI 

XMAX 

YMAX 

GQ4(1) 

GQ4(2) 

GQ4(3) 

NQ1 

0.3 

0.1 

1 

0.1 

0.3 

- 0.4 

0.2 

NQ2 

0.4 

0.1 

1 

0.14 

0.4 

- 0.48 

0.22 

NQ3 

0.4 

0.1 

1 

0.18 

0.4 

- 0.36 

0.14 

NQ4 

0.5 

0.1 

1 

0.18 

0.5 

- 0.56 

0.24 

NQ5 

0.5 

0.1 

1 

0.21 

0.5 

- 0.47 

0.18 


SURGE 



SO 

SI 

XMAX 

YMAX 

GX4(1) 

GX4(2) 

GX4(3) 

NX1 

0.5 

0.1 

10 

2 

0.5 

- 0.05 

0.002 

NX2 

0.5 

0.1 

10 

2.5 

0.5 

- 0.035 

0.001 

NX3 

0.6 

0.1 

10 

2.5 

0.6 

- 0.055 

0.002 

NX4 

0.7 

0.1 

10 

2.5 

0.7 

- 0.075 

0.003 

NX5 

0.7 

0.1 

10 

3 

0.7 

- 0.06 

0.002 


SWAY 



SO 

SI 

XMAX 

YMAX 

GY4(1) 

GY4(2) 

GY4(3) 

NY1 

0.3 

0 

10 

1.2 

0.3 

- 0.024 

0.0006 

NY2 

0.4 

0 

10 

1.2 

0.4 

- 0.044 

0.0016 

NY3 

0.4 

0 

10 

1.5 

0.4 

- 0.035 

0.001 

NY4 

0.5 

0 

10 

1.5 

0.5 

- 0.055 

0.002 

NY5 

0.5 

0 

10 

1.8 

0.5 

- 0.046 

0.0014 


HEAVE (IN-AIR) 



SO 

SI 

XMAX 

YMAX 

GZ40(1) 

GZ40(2) 

GZ40(3) 

NZ1 

0.6 

0.1 

10 

1.6 

0.6 

- 0.082 

0.0038 

NZ2 

0.6 

0.1 

10 

2 

0.6 

- 0.07 

0.003 

NZ3 

0.7 

0.1 

10 

2 

0.7 

- 0.09 

0.004 

NZ4 

0.7 

0.1 

10 

2.4 

0.7 

- 0.078 

0.0032 

NZ5 

0.8 

0.1 

10 

2.4 

0.8 

- 0.098 

0.0042 


HEAVE (ON-GROUND) 



SO 

SI 

XMAX 

YMAX 

GZ4S(1) 

GZ4S(2) 

GZ4S(3) 

NL1 

1.6 

0 

20 

20 

1.6 

- 0.010 

- 0.0010 

NL2 

1.8 

0 

20 

20 

1.8 

- 0.030 

- 0.0005 

NL3 

2 

0 

20 

20 

2 

- 0.050 

0.0000 

NL4 

2.2 

0 

20 

20 

2.2 

- 0.070 

0.0005 

NL5 

2.4 

0 

20 

20 

2.4 

- 0.090 

0.0010 
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Prior to adjustment of individual degree-of-freedom gain coefficient sets, it is 

suggested that an experienced simulator pilot execute a series of maneuvers to determine 

the baseline pilot performance for the current gain set, noting deficiencies for each 

degree-of-freedom that may arise from specific maneuvers. From prior piloted tuning 

tests, the following maneuvers are recommended for large transport aircraft: 

Pitch Stick, Roll Stick, and Rudder Pedal Doublets 
Throttle Increase and Decrease 
Takeoff from Full Stop 

Straight Approach and Landing (with and without turbulence) 

Coordinated Turn 

Offset Approach and Landing 

Ground Maneuvers (taxiing, effect of aircraft brakes). 

Table 5.3 lists the maneuvers, the degrees of freedom that require tuning for the 
maneuver, and the desired outcomes that result from the tuning. The maneuvers should 
be executed in the order listed in Table 5.3. Adjustment of gains for individual degrees 
of freedom should be done repeatedly for each maneuver. In general, individual degree- 
of-freedom gain coefficient sets should be adjusted until the desired motion cues and 
pilot performance result for each maneuver, while ensuring that the platform does not 
exceed the actuator extension limits. Following execution of the maneuvers with tuning 
of the degree-of-freedom nonlinear gain sets, the simulator pilot should repeat execution 
of all maneuvers, noting any final deficiencies in the motion cues and piloted 
performance that may persist. If necessary, additional fine tuning of any degree-of- 
freedom can then be performed. 

The augmented turbulence gain K G given in Figure 2.10 can be increased or 
decreased to produce the desired turbulence cues within the capabilities of the motion 
platform. An adjustment of K a may also be sufficient with a change of the aircraft 
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dynamics; a large transport aircraft similar to a Boeing 757-200 should produce similar 
turbulence cues. The transfer function time constants given in Eq. (2.8) can also be 
adjusted to produce the desired motion cues for greater changes in aircraft dynamics. 
The lead time constants in the numerator can be increased to increase the magnitude of 
the low-frequency cues. Decreasing the time constants in the denominator will increase 
the magnitude of the high-frequency cues, but these terms should be large enough so that 
the transfer function can be integrated within the simulation time step. 


Table 5.3. Maneuvers with Degree-of-Freedom Gains. 


Maneuver 

Degree-of-Freedom 

Comments 




Pitch Stick Doublet 

Pitch 

Adjust gains to obtain desired cues 

Roll Stick Doublet 

Roll/Yaw 

Adjust gains to obtain desired cues 

Rudder Pedal Doublet 

Roll/Yaw 

Adjust gains to obtain desired cues 

Throttle 

Increase/Decrease 

Surge/Heave 

Adjust gains to obtain desired cues 

Takeoff 

Pitch/Surge/Heave 

Ease in control of takeoff to desired pitch 
attitude, forward surge cues should be 
coordinated with pitch cues, vertical cues 
noticeable, ensure pitch and heave gains do 
not exceed platform limits 

Straight-In Approach 

Pitch/S urge/He ave 

Ease in control of approach, forward surge 
cues should be coordinated with pitch cues, 
vertical cues noticeable 

Coordinated Turn 

Roll/Sway/Yaw 

Minimize side force by reducing roll 
and/or increasing sway cues, ensure roll 
and sway gains do not exceed platform 
limits 

Offset Approach 

Roll/Sway/Yaw 

Ease in control of approach, forward surge 
cues should be coordinated with pitch cues, 
minimal side forces 

Turbulence 

Heave 

Adjust augmented turbulence gains to 
obtain desired turbulence sensation 

Landing Touchdown 

On-Ground Heave 

Increase on-ground heave gain to obtain 
desired touchdown sensation, ensure 
motion platform limits are not exceeded 

Ground Maneuvers 

On-Ground Heave 

Increase on-ground heave gain to obtain 
desired ground motion sensation, ensure 
motion platform limits are not exceeded 
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6. Suggested Future Implementation 

Reducing the nonlinear gains for the optimal algorithm (surge) and the nonlinear 
algorithm (surge and pitch) was necessary so that the takeoff maneuver could be flown 
within the 60-inch actuator extension limits and low bandwidth (2-Hz) of the Langley 
Visual Motion Simulator (VMS). However, gain reductions contributed to degradation in 
pilot performance that was observed most frequently with the straight-in approach. 
These results were observed during piloted performance tests conducted with the optimal 
and nonlinear motion cueing algorithms [12]. Implementation of the optimal and 
nonlinear algorithms on a platform with increased actuator extensions would allow for 
increased gains, thus resulting in improved pilot performance. One such motion platform 
is located in the Cockpit Motion Facility (CMF) [13], shown in Figure 6.1, presently 
being erected at the NASA Langley Research Center. 


Cockpit Motion Facility 
(CMF) 



Figure 6.1. NASA Langley Cockpit Motion Facility (CMF). 
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The Cockpit Motion Facility is made up of one motion system site and four fixed- 
base sites. The motion system site contains a six-degree-of-freedom state-of-the-art 
synergistic motion base with 76-inch actuator extensions. The four fixed-base sites allow 
the simulator cockpits to be used in fixed-base mode when they are not resident on the 
motion system. Each cockpit has its own visual display system and all cockpits share 
Evans and Sutherland ESIG 3000 image generators. 

Pilot tests with the optimal algorithm [12] revealed a number of cases in which 
the braking algorithm arrested the motion, but with the braking algorithm being unable to 
recover the motion. In one case, an experienced military aircraft pilot exceeded the 
motion system actuator limits on several test runs due to aggressive liftoff rotations that 
resulted in a very high rate of climb and large heave displacement. Similar tests revealed 
that the nonlinear algorithm was less likely to enter the braking region, due to the fact 
that the algorithm progressively reduces the platform displacements for large inputs; thus 
resulting in a larger motion envelope than the optimal algorithm. However, similar 
problems persisted with the braking algorithm when simulator motion was restrained, 
with the braking algorithm failing to recover to normal motion. Based upon these results, 
the braking algorithm is not performing its desired function and future improvements 
need to be investigated. An improved algorithm that is effective in both restraining large 
excursions and resuming regular simulator motion would allow increased nonlinear gains 
and improve motion cueing performance. One suggested approach is the algorithm 
developed by McFarland [14] for NASA Ames. 

The nonlinear algorithm will ultimately be implemented on the CMF. Pilot 
tuning of the nonlinear gains, similar to that previously done for the new algorithms on 
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the VMS as discussed in Section 5.1, will be performed. The tuning procedure given in 
Section 5.2 can be followed. The gain coefficient sets given in Table 5.2 were set up for 
tuning the CMF with increased actuator extensions in comparison to the VMS. It is 
expected that the CMF will accommodate increased gains for the pitch, surge, and 
possibly the heave degrees of freedom. Due to the algorithm producing faster washout 
with large motion cues, the necessity for a braking algorithm to address large excursions 
may be minimal. It is suggested that the nonlinear algorithm be first implemented and 
tuned without a braking algorithm, with a new braking algorithm to be developed if 
needed based on that performance. 
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7. Common Block Variable Listings 


7.1. comint2.com (Variables Used in Optimal and Nonlinear Algorithms) 


Variable 

Description 



X, Y, Z 

Desired Platform Displacements (m) 

XD, YD, ZD 

Desired Platform Velocities (m/s) 

XDD, YDD, ZDD 

Desired Platform Accelerations (m/s/s) 

PHI, THE, PSI 

Desired Platform Attitudes (rad) 

PHID, THED, PSID 

Desired Platform Angular Velocities (rad/s) 


7.2. optint3.com 


Variable 

Description 



DT 

Time Step (Sec) 

XX1(8), X10(8) 

Filter State Variable 1 (Current and Prior Time) 

X2(8), X20(8) 

Filter State Variable 2 (Current and Prior Time) 

X3(8), X30(8) 

Filter State Variable 3 (Current and Prior Time) 

X4(8), X40(8) 

Filter State Variable 4 (Current and Prior Time) 

X5(6), X50(6) 

Filter State Variable 5 (Current and Prior Time) 

X6(6), X60(6) 

Filter State Variable 6 (Current and Prior Time) 

ACA(3), ACAO(3) 

Aircraft Body Acceleration Vector (m/s/s) 

BETAA(3) 

Aircraft Euler Angle Vector (rad) 

BETAAD(3) 

Aircraft Euler Rate Vector (rad/sec) 

A2(3) 

Aircraft Inertial Acceleration Vector (m/s/s) 

A2N(3), A2NO(3) 

Scaled Aircraft Inertial Acceleration Vector (m/s/s) 

BADN(3), BADNO(3) 

Scaled Aircraft Euler Rate Vector (rad/s) 

ASI(3), ASIO(3) 

Desired Platform Acceleration Cue (m/s/s) 

VSI(3), VSIO(3) 

Desired Platform Velocity (m/s) 

SSIU(3), SSI1(3), SSIO(3) 

Desired Platform Displacement (m) 

WAA(3) 

Aircraft Body Velocity Vector (rad/sec) 

BSDT(2), BSDTO(2), BSDTL(2) 

Platform Tilt Cue (Current, Prior, and Fimited) (rad/sec) 

BETAS 1(3) 

Desired Platform Angular Position (rad) 

BDLIM 

Platform Tilt Cue Limit (rad/sec) 

BSDR(3), BSDRO(3) 

Platform Rotational Cue (Current and Prior) (rad/sec) 

BETASR(3), BETASRO(3) 

Platform Rotational Angle (Current and Prior) (rad/sec) 

BETAST(2), BETASTO(2) 

Platform Tilt Angle (Current and Prior) (rad) 

BETASTL(2), BETASTLO(2) 

Platform Tilt Angle with Limit (Current and Prior) (rad) 

DIF(2) 

Difference between Current and Limited Tilt Angles 

XH(2), XHO(2) 

Trim Filter State Variables 

T1N1, T1D0, T2N1, T2D0 

Trim Filter Coefficients 

XT, XTO, XT2, XT20 

Augmented Turbulence Filter State Variables 

ACZT 

Augmented Turbulence Acceleration 

WGUST, WGUSTO 

Z-Axis Gust Velocity (m/sec) 

G1D0, G1D1 

Augmented Turbulence Filter Coefficients (Denominator) 

G1N0, G1N1, G1N2 

Augmented Turbulence Filter Coefficients (Numerator) 
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7.3. wopt3.com 


Variable 

Description 



R1N6,R1N5,R1N4,R1N3,R1N2,R1N1,R1N0 

Lateral Filter W u Numerator Coefficients 

R2N6,R2N5,R2N4,R2N3,R2N2,R2N1,R2N0 

Lateral Filter W 21 Numerator Coefficients 

R4N6,R4N5,R4N4,R4N3,R4N2,R4N1,R4N0 

Lateral Filter W 22 Numerator Coefficients 

R1D5,R1D4,R1D3,R1D2,R1D1,R1D0 

Lateral Filter Denominator Coefficients 

P1N6,P1N5,P1N4,P1N3,P1N2,P1N1,P1N0 

Longitudinal Filter W u Numerator Coefficients 

P2N6,P2N5,P2N4,P2N3,P2N2,P2N 1 ,P2N0 

Longitudinal Filter W 21 Numerator Coefficients 

P4N6,P4N5,P4N4,P4N3,P4N2,P4N1,P4N0 

Longitudinal Filter W 22 Numerator Coefficients 

P1D5,P1D4,P1D3,P1D2,P1D1,P1D0 

Longitudinal Filter Denominator Coefficients 

Y1N4,Y1N3,Y1N2,Y1N1,Y1N0 

Yaw Filter Numerator Coefficients 

Y1D3,Y1D2,Y1D1,Y1D0 

Yaw Filter Denominator Coefficients 

H1N4,H1N3,H1N2,H1N1,H1N0 

Vertical Filter Numerator Coefficients 

H1D3,H1D2,H1D1,H1D0 

Vertical Filter Denominator Coefficients 

GX3,GY3,GZ3,GP3,GQ3,GR3 

In-Flight Polynomial Scaling Coefficients 

AMX3.BMX3 

In-Flight Translational and Rotational Limits 

GZ30.GZ3S.AMX30.AMX3S 

On-Ground Scaling Coefficients and Limits 

GT2 

Augmented Turbulence Acceleration Gain 


7.4. matrixlc.com 


Variable 

Description 



AAIS(3,6) 

Motion Base Actuator Coordinates (m) 

BBII(3,6) 

Fixed Base Actuator Coordinates (m) 

RRS(3) 

Vector from Motion Base Centroid to Pilot Head (m) 

LENEUT 

Actuator Neutral Length (m) 

LI(6) 

Desired Actuator Extensions (m) 

RLI(6), RLIO(6) 

Actual Actuator Extensions (m) 

LEGC(6) 

Actual Actuator Extensions (in) 

SSI(3) 

Actual Simulator Displacements (m) 

BETAS (3) 

Actual Simulator Attitudes (rad) 

BRAKE(6) 

Actuator Braking Region V alue 

RATIO(6) 

Actuator Braking Ratio 

LA VAIL 

Actuator Available Length (m) 

RLID(6). RLIDO(6) 

Actuator Velocity (m/s) 

RLIDDI6) 

Actuator Acceleration (m/s/s) 

FLAG(6) 

Braking Region Flag (0 or 1) 

FLAG2 

Braking Region Flag (0 or 1) 

IT2, NC2 

Braking Recovery Indices 

SUMFLAG 

Sum of FLAG(6) Values 
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7.5. nopt4.com 


Variable 

Description 



Longitudinal Mode 


ALPX, ALPXMAX 

Prescribed Nonlinearity a 

APX(11,11), APXO(ll) 

State Space System Matrix A' 

BRBX(1 1,11), RlPX(lLll), R2IX(2) 

System Weighting Matrix S, R/, R 2 

AVX(6,6), BVX(6,2), DQCX(6) 

Vestibular State Space Matrices Ay, B v , D v QCy 

K1X(2,6), K2X(2,3), K3X(2) 

Feedback Matrices Ki, K 2 , K 3 

ZX(11,11) 

Neurocomputing Solver Bi-Polar Vectors Z 

PX( 11,11), PXVEC(66) 

Riccati Equation Solution P 

XX(9), XXO(9) 

State Vector x 



Lateral Mode 


ALPY, ALPYMAX 

Prescribed Nonlinearity a 

APY(1 1,1 1), APYO(ll) 

State Space System Matrix A' 

BRBY(11,11). R1PX(11,11), R2IY(2) 

System Weighting Matrix S, R/, R 2 

AVY(6,6), BVY(6,2), DQCY(6) 

Vestibular State Space Matrices Ay, B v , D v QCy 

K1Y(2,6), K2Y(2,3), K3Y(2) 

Feedback Matrices K b K 2 , K 3 

ZY(11,11) 

Neurocomputing Solver Bi-Polar Vectors Z 

PY( 11,11), PYVEC(66) 

Riccati Equation Solution P 

XY(9), XYO(9) 

State Vector x 



Yaw Mode 


ALPR, ALPRMAX 

Prescribed Nonlinearity a 

APR(5,5), APRO(5) 

State Space System Matrix A' 

BRBR(5,5), R1PR(5,5), R2IR 

System Weighting Matrix S, R/, R 2 

AVY(3,3), BVR(3), DQCR(3), DQDR 

Vestibular State Space Matrices A v , B v , D V QC V , D V QD V 

K1R(3), K2R, K3R 

Feedback Matrices K b K 2 , K 3 

ZR(5,5) 

Neurocomputing Solver Bi-Polar Vectors Z 

PR(5,5), PRVEC(15) 

Riccati Equation Solution P 

XR(4), XRO(4) 

State Vector x 



Heave Mode 


ALPZ, ALPZMAX 

Prescribed Nonlinearity a 

APZ(6,6), APZO(6) 

State Space System Matrix A' 

BRBZ(6,6), R1PZ(6,6) 

System Weighting Matrix S, R/ 

AVZ(2,2), BVZ(2) 

Vestibular State Space Matrices A v , B v 

K1Z(2), K2Z(2), K3Z 

Feedback Matrices K l5 K 2 , K 3 

ZZ(6,6) 

Neurocomputing Solver Bi-Polar Vectors Z 

PZ(6,6), PZVEC(21) 

Riccati Equation Solution P 

XZ(5), XZO(5) 

State Vector x 



Nonlinear Gains and Turbulence 


GX4,GY4,GZ4,GP4,GQ4,GR4 

In-Flight Polynomial Scaling Coefficients 

AMX4.BMX4 

In-Flight Translational and Rotational Limits 

GZ40,GZ4S,AMX40,AMX4S 

On-Ground Scaling Coefficients and Limits 

GT4 

Augmented Turbulence Acceleration Gain 

G2D0, G2D1 

Augmented Turbulence Filter Coefficients (Denominator) 

G2N0, G2N1, G2N2 

Augmented Turbulence Filter Coefficients (Numerator) 
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8. Program Listing 

8.1. gainopt3.f 


Q ★ -k -k -k -k -k -k -k -k -k -k -k -k -k ic -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k 

C OPTIMAL ALGORITHM NONLINEAR GAIN SUBROUTINE. 

C THE INPUT IS FIRST LIMITED AND THEN SCALED BY A POLYNOMIAL. 

c 

SUBROUTINE GAINOPT3 

INCLUDE ' optint3 . com' 

INCLUDE ' wopt3.com' 

REAL AA ( 3 ) , BA ( 3 ) 

C 

C Take Absolute Value of Input 

C 

AA ( 1 ) =ABS (A2 (1) ) 

AA ( 2 ) =ABS (A2 (2) ) 

AA ( 3 ) =ABS (A2 (3) ) 

BA ( 1 ) =ABS (BETAAD (1) ) 

BA (2) =ABS (BETAAD (2) ) 

BA (3) =ABS (BETAAD (3) ) 

C 

C Limit Translational and Rotational Inputs 

C 

AAM=MAX ( AA (1),AA(2),AA(3)) 

BAM=MAX (BA ( 1 ) , BA ( 2 ) , BA ( 3 ) ) 

IF (AAM.GT.AMX3) THEN 
RATI0=AMX3 / AAM 
AA ( 1 ) =AA ( 1 ) * RATIO 
AA ( 2 ) =AA ( 2 ) * RATIO 
AA ( 3 ) =AA ( 3 ) * RATIO 
END IF 

IF (BAM.GT.BMX3) THEN 
RATI0=BMX3/BAM 
BA ( 1 ) =BA ( 1 ) * RATIO 
BA (2) =BA ( 2 ) * RATIO 
BA (3) =BA ( 3 ) * RATIO 
END IF 
C 

C Perform Nonlinear Scaling of Inputs 

C 

A2N ( 1 ) = (GX3 (1) *AA ( 1 ) +GX3 (2) *AA(1) **2 . +GX3 (3) *AA(1) **3 . ) 
+*SIGN (1 . , A2 (1) ) 

A2N (2) = (GY3 (1) *AA(2) +GY3 (2) *AA(2) **2 . +GY3 (3) *AA(2) **3 . ) 
+*SIGN (1 . , A2 (2) ) 

A2N (3) = (GZ3 (1) *AA ( 3 ) +GZ3 (2) *AA(3) **2 . +GZ3 (3) *AA(3) **3 . ) 
+*SIGN ( 1 . , A2 (3) ) 

BADN (1) = (GP3 (1) *BA ( 1 ) +GP3 (2) *BA(1) **2 . +GP3 (3) *BA(1) **3 . ) 
+*SIGN ( 1 . , BETAAD (1) ) 

BADN (2) = (GQ3 (1) *BA(2) +GQ3 (2) *BA(2) **2 . +GQ3 (3) *BA(2) **3 . ) 
+*SIGN ( 1 . , BETAAD (2) ) 

BADN (3) = (GR3 (1) *BA(3) +GR3 (2) *BA(3) **2 . +GR3 (3) *BA(3) **3 . ) 
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+*SIGN ( 1 . , BETAAD (3 ) ) 


RETURN 

END 
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8.2. gainopt4.f 


Q9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9r9c , 9c , 9r9c , 9r 

C NONLINEAR ALGORITHM NONLINEAR GAIN SUBROUTINE. 

C THE INPUT IS FIRST LIMITED AND THEN SCALED BY A POLYNOMIAL. 

C 

SUBROUTINE GAINOPT4 

INCLUDE 'optint3.com' 

INCLUDE 'nopt4.com' 

REAL AA4 ( 3 ) , BA4 ( 3 ) 

C 

C Take Absolute Value of Input 
C 

AA4 (1) =ABS (A2 (1) ) 

AA4 (2) =ABS (A2 (2) ) 

AA4 (3) =ABS (A2 (3) ) 

BA4 (1) =ABS (BETAAD (1) ) 

BA4 (2) =ABS (BETAAD (2) ) 

BA4 (3) =ABS (BETAAD (3) ) 

C 

C Limit Translational and Rotational Inputs 
C 

AAM=MAX (AA4 ( 1 ) , AA4 ( 2 ) , AA4 ( 3 ) ) 

BAM = MAX (BA4 (1) , BA4 (2) , BA4 (3) ) 

IF (AAM.GT.AMX4) THEN 
RATI0=AMX4/AAM 
AA4 (1) =AA4 (1) * RATIO 
AA4 (2) =AA4 (2) *RATIO 
AA4 (3) =AA4 (3) *RATIO 
END IF 

IF (BAM.GT.BMX4) THEN 
RATI0=BMX4/BAM 
BA4 (1) =BA4 (1) * RATIO 
BA4 (2) =BA4 (2) *RATIO 
BA4 (3) =BA4 (3) *RATIO 
END IF 
C 

C Perform Nonlinear Scaling of Inputs 

C 

A2N (1) = (GX4 (1) *AA4 (1) +GX4 (2) *AA4 (1) **2 . +GX4 (3) *AA4 (1) **3 . ) 
+*SIGN (1 . , A2 (1) ) 

A2N (2) = (GY4 (1) *AA4 (2) +GY4 (2) *AA4 (2) **2 . +GY4 (3) *AA4 (2) **3 . ) 
+*SIGN (1 . , A2 (2) ) 

A2N (3) = (GZ4 (1) *AA4 (3) +GZ4 (2) *AA4 (3) **2 . +GZ4 (3) *AA4 (3) **3 . ) 
+*SIGN ( 1 . ,A2 (3) ) 

BADN (1) = (GP4 (1) *BA4 (1) +GP4 (2) *BA4 (1) **2 . +GP4 (3) *BA4 (1) **3 . ) 
+*SIGN ( 1 . , BETAAD (1) ) 

BADN (2) = (GQ4 (1) *BA4 (2) +GQ4 (2) *BA4 (2) **2 . +GQ4 (3) *BA4 (2) **3 . ) 
+*SIGN ( 1 . , BETAAD (2 ) ) 

BADN (3) = (GR4 (1) *BA4 (3) +GR4 (2) *BA4 (3) **2 . +GR4 (3) *BA4 (3) **3 . ) 
+*SIGN ( 1 . , BETAAD (3) ) 
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RETURN 

END 
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8.3. integ3.f 

SUBROUTINE INTEG3 
INCLUDE ' optint3 . com' 
INCLUDE 1 comint 2 . com ' 
INCLUDE 'wcom2.com' 
INCLUDE 'matrixlc.com' 


C 

C INTEGRATE : 

C SIMULATOR LINEAR ACCEL TO LINEAR DISPL 

C SIMULATOR ANGULAR VEL TO ANGULAR DISPL 

C 

DO K=1 , 3 

VSI (K) =VSIO (K) +DT* 0 . 5* (ASI (K) +ASIO (K) ) 

SSIU(K) =SSIO (K) +DT* 0 . 5* (VSI (K) +VSIO (K) ) 

BETASR (K) =BETASRO (K) +DT*0 . 5* (BSDR (K) +BSDRO (K) ) 

ASIO (K) =ASI (K) 

VSIO (K) =VSI (K) 

SSIO (K) =SSIU(K) 

BSDRO (K) =BSDR (K) 

BETASRO (K) =BETASR (K) 

END DO 
C 

C LIMIT THE ANGULAR RATE IN THE CROSS-OVER TILT CHANNEL. 

C THE REAL TILT POSITION WILL BE DETERMINED BY BOTH THE DESIRED 

C POSITION AND THE DIFFERENCE BETWEEN THE DESIRED AND REAL 

C TILT POSITION. 

C 

DO K=1 , 2 

BETAST (K) =BETASTO (K) +DT*0 . 5* (BSDT (K) +BSDTO (K) ) 

DIF (K) =0.005* (BETAST (K) -BETASTLO (K) ) + (BETAST (K) -BETASTO (K) ) 
BETASTL (K) =BETASTLO (K) +MAX ( -BDLIM*DT , MIN (BDLIM*DT , DIF (K) ) ) 

C 

C COMPUTE THE TILT ANGULAR VELOCITY 

C 

BSDTL (K) = (BETASTL (K) -BETASTLO (K) ) /DT 

BETASTO (K) =BETAST (K) 

BETASTLO (K) =BETASTL (K) 

BSDTO (K) =BSDT (K) 

END DO 
C 

C COMBINE THE TILT AND ROTATIONAL CHANNELS TO OBTAIN 

C THE DESIRED ANGULAR POSITION 

C 

BETAS 1 ( 1 ) =BETASTL ( 1 ) +BETASR ( 1 ) 

BETAS 1 (2) =BETASTL (2) +BETASR (2 ) 

BETAS 1 (3) =BETASR (3 ) 

C 

C USE DIFFERENCE BETWEEN DESIRED BETAST AND REAL BETAST 
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C TO GENERATE ADDITIONAL LINEAR RESPONSE AND ACHIEVE 

C COORDINATION BETWEEN THE LINEAR AND TILT CHANNELS 

C 

SSI1 (1) =SSIU(1) +RRS (3) * (BETAST (2) -BETASTL (2 ) ) 

SSI1 (2) =SSIU(2) -RRS (3) * (BETAST (1) -BETASTL (1) ) 

C 

C FOR ON-GROUND MOTION, ADD RUNWAY ROUGHNESS EFFECT 

C AMPLITUDE IS FAIRED UPON TOUCHDOWN OR TAKEOFF 

C 

SSI1 (3) =SSIU(3) +XKA*SIN ( (WB+XKG*VGSPD) *T) 

C 

C Swap Variables To Match Modified Algorithm 

C For Input to JACKDRVR 

C 

XDD = AS I (1) 

YDD = AS I (2) 

ZDD = AS I (3) 

XD = VSI (1) 

YD = VSI (2) 

ZD = VSI (3) 

X = SSI1 (1) 

Y = SSI1 (2) 

Z = SSI1 (3) 


PHI = 

BETAS 1 (1) 

THE = 

BETAS 1 (2) 

PSI = 

BETAS 1 (3) 

PHID 

= BSDTL ( 1 ) +BSDR ( 1 ) 

THED 

= BSDTL (2) +BSDR (2) 

PSID 

= BSDR ( 3 ) 

RETURN 

END 
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8.4. integ4.f 

SUBROUTINE INTEG4 

INCLUDE 'optint3.com' 

INCLUDE 'comint2.com' 

INCLUDE 'wcom2.com' 

INCLUDE 'matrixlc.com' 

INCLUDE 'nopt4.com' 

C 

C SURGE FILTER OUTPUT ASI(l) & BSDT(2) 

C 

ASI (1) =-KlX (2,1) *XX (1) -K1X (2,2) *XX (2) -K1X (2,3) *XX (3) 
x -K1X (2,4) *XX (4) -K1X (2,5) *XX (5) -K1X (2, 6) *XX (6) 

x -K2X (2,1) *XX (7) -K2X (2,2) *XX (8) -K2X (2, 3) *XX (9) 

x -K3X (2) *A2N (1) 

BSDT (2) = -K1X ( 1 , 1) *XX (1) -K1X (1, 2) *XX (2) -K1X (1, 3) *XX (3) 
x -K1X (1, 4) *XX (4) -K1X (1, 5) *XX (5) -K1X (1, 6) *XX (6) 

x -K2X (1, 1) *XX (7) -K2X (1, 2) *XX (8) -K2X (1, 3) *XX (9) 

x -K3X ( 1 ) *A2N ( 1 ) 

C 

C SWAY FILTER OUTPUT ASI (2) & BSDT(l) 

C 

ASI (2) =-KlY (2 , 1) *XY (1) -K1Y (2, 2) *XY (2) -K1Y(2, 3) *XY (3) 
x -K1Y (2,4) *XY (4 ) -K1Y(2, 5) *XY(5) -K1Y(2, 6) *XY(6) 

x -K2Y (2 , 1) *XY (7) -K2Y (2, 2) *XY (8) -K2Y (2, 3) *XY (9) 

x -K3Y (2) *A2N (2) 

BSDT (1) =-KlY (1, 1) *XY (1) -K1Y (1, 2) *XY (2) -K1Y (1, 3) *XY (3) 
x -K1Y (1, 4) *XY (4) -K1Y (1, 5) *XY (5) -K1Y (1, 6) *XY (6) 

x -K2Y (1, 1) *XY (7) -K2Y (1, 2) *XY (8) -K2Y (1, 3) *XY (9) 

x -K3Y (1) *A2N (2) 

C 

C HEAVE FILTER OUTPUT ASI (3) 

C 

ASI (3) = ( -K1Z (1) *XZ (1) -K1Z (2) *XZ (2) -K2Z (1) *XZ (3) 
x -K2Z (2 ) *XZ (4) -K2Z (3) *XZ (5) -K3Z*A2N(3) ) 

C 

C YAW FILTER OUTPUT BSDR(3) 

C 

BSDR ( 3 ) = -KIR ( 1 ) *XR ( 1 ) -KIR (2) *XR(2) -KIR (3) *XR(3) 
x -K2R*XR (4 ) -K3R*BADN (3 ) 

C 

C LIMIT THE ANGULAR RATE IN THE CROSS-OVER TILT CHANNEL. 

C THE REAL TILT POSITION WILL BE DETERMINED BY BOTH THE DESIRED 

C POSITION AND THE DIFFERENCE BETWEEN THE DESIRED AND REAL 

C TILT POSITION. 

C 

DO K=1 , 2 

BETASR (K) =BETASRO (K) +DT*BSDRO (K) 

BETAST (K) =BETASTO (K) +DT*BSDTO (K) 
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DIF (K) =0.005* (BETAST (K) -BETASTLO (K) ) + (BETAST (K) -BETASTO (K) ) 
BETASTL (K) =BETASTLO (K) +MAX ( -BDLIM*DT , MIN (BDLIM*DT , DIF (K) ) ) 

C 

C COMPUTE THE TILT ANGULAR VELOCITY 

C 

BSDTL (K) = (BETASTL (K) -BETASTLO (K) ) /DT 
BSDRO(K) = BSDR(K) 

BETASRO (K) =BETASR (K) 

BETASTO (K) =BETAST (K) 

BETASTLO (K) =BETASTL (K) 

BSDTO (K) =BSDT (K) 

END DO 
C 

C COMBINE THE TILT AND ROTATIONAL CHANNELS TO OBTAIN 

C THE DESIRED ANGULAR POSITION 

C 

BETAS 1 ( 1 ) =BETASTL ( 1 ) +BETASR ( 1 ) 

BETAS 1 (2) =BETASTL (2) +BETASR (2 ) 

BETAS 1 (3) =XR (4 ) 

C 

C USE DIFFERENCE BETWEEN DESIRED BETAST AND REAL BETAST 
C TO GENERATE ADDITIONAL LINEAR RESPONSE AND ACHIEVE 
C COORDINATION BETWEEN THE LINEAR AND TILT CHANNELS 
C 

SSI1 (1) =XX (8) +RRS (3) * (BETAST (2) -BETASTL (2) ) 

SSI1 (2) =XY (8) -RRS (3) * (BETAST (1) -BETASTL (1) ) 

C 

C FOR ON-GROUND MOTION, ADD RUNWAY ROUGHNESS EFFECT 

C AMPLITUDE IS FAIRED UPON TOUCHDOWN OR TAKEOFF 

C 

SSI1 (3) =XZ (4) +XKA*SIN( (WB+XKG*VGSPD) *T) 

C 

C Swap Variables To Match Modified Algorithm 

C For Input to JACKDRVR 

C 

XDD = AS I (1) 

YDD = AS I (2) 

ZDD = AS I (3) 

XD = XX ( 9 ) 

YD = XY ( 9 ) 

ZD = XZ ( 5 ) 

X = SSI1 (1) 

Y = SSI1 (2) 

Z = SSI1 (3) 


PHI = 

BETAS 1 (1) 

THE = 

BETAS 1 (2) 

PSI = 

BETAS 1 (3) 

PHID 

= BSDTL (1) +BSDR (1) 

THED 

= BSDTL (2) +BSDR(2) 

PSID 

= BSDR ( 3 ) 

RETURN 

END 
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8.5. invplf.f 


C THIS SUBROUTINE WILL UNDERTAKE AN INVERSE TRANSFORMATION DEVELOPED 
C BY THE NEWTON- RAPHSON TECHNIQUE. LEG EXTENSIONS WILL BE TRANSFORMED 
C TO THE DEGREES OF FREEDOM. THIS INVERSE TRANSFORMATION IS PERFORMED 
C BY AN ITERATIVE METHOD DENOTED AS NEWTON- RAPHSON TECHNIQUE. 

C ITERATIONS ARE TERMINATED WHEN THE DIFFERENCE BETWEEN TWO SUBSEQUENT 
C ITERATIONS IS LESS THAN SOME ERROR CRITERION. 

C RLI IS THE LEG EXTENSIONS. 

C SSI IS THE TRANSLATIONAL DISPLACEMENT OF THE PLATFORM. 

C BETAS IS THE ANGULAR DISPLACEMENT OF THE PLATFORM. 

C XS , YS , ZS : COORDINATES OF THE FIXED ENDS OF THE LEGS. 

C XM , YM , ZM : COORDINATES OF THE MOVING ENDS OF THE LEGS. 

C AAIS , BBI I : GEOMETRY OF THE MOTION SYSTEM. 

C SSIIN: INITIAL TRANSLATIONAL DISPLACEMENT. 

C RRS : VECTOR NOT USED BY THIS SUBROUTINE. 

C LENEUT: LENGTH OF LEGS IN NEUTRAL POSITION. 

C 

SUBROUTINE INVPLF 

REAL RAML ( 6 ) , XS ( 6 ) , YS ( 6 ) ( ZS ( 6 ) , XM ( 6 ) , YM ( 6 ) , ZM ( 6 ) 

REAL F ( 6 ) , PFX ( 6 ) , PFY ( 6 ) , PFZ (6) , PFS ( 6 ) , PFT (6) , PFP ( 6 ) , 

+ A ( 3 , 3 ) , ZEBRA (36) ,P 

INCLUDE 'matrixlc.com' 

DATA I FLAG/ 0/ 

(2 it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it 

C INITIALIZE SIMULATOR POSITION. 

DATA X/0 . /, Y/0 . /, Z/0 . /, P/0 . /,T/0 . /, S/0 . / 

Q it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it it 

c 

SAVE 

IF ( I FLAG . EQ . 0 ) THEN 
DO JACK=1 , 6 

XM (JACK) =AAIS (1, JACK) 

YM (JACK) =AAIS (2, JACK) 

ZM (JACK) =AAIS (3 , JACK) 

XS (JACK) =BBI I (1, JACK) 

YS (JACK) =BBI I (2, JACK) 

ZS (JACK) =BBI I (3 , JACK) 

END DO 
IFLAG=1 
END IF 

(2 it it it it it it it it it it it it it it it it it it it it it it it 

C X=SSI (1) +SSIIN (1) 

C Y=SSI (2) +SSIIN (2) 

C Z=SSI (3) +SSIIN(3) 

C P=BETAS ( 1 ) 

C T=BETAS (2 ) 

C S=BETAS ( 3 ) 

(2 it it it it it it it it it it it it it it it it it it it it it it it 

DO JACK=1 , 6 

RAML (JACK) =RLI (JACK) +LENEUT 
END DO 
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IT=0 

9 CONTINUE 

A ( 1 , 1 ) =COS (S) *COS (T) 

A ( 1 , 2 ) =SIN (S) *COS (T) 

A ( 1 , 3 ) = -SIN (T) 

A (2, 1) =COS (S) *SIN (T) *SIN (P) -SIN (S) *COS (P) 

A (2, 2) =SIN (S) *SIN (T) *SIN (P) +COS (S) *COS (P) 

A ( 2 , 3 ) =COS (T) *SIN (P) 

A (3 , 1) =COS (S) *SIN (T) *COS (P) +SIN ( S ) *SIN ( P) 

A (3 , 2) =SIN (S) *SIN (T) *COS (P) -COS (S) *SIN(P) 

A ( 3 , 3 ) =COS (T) *COS (P) 

DO 17 1=1,6 

F (I) =XM (I) **2 . +YM (I) **2 . +ZM (I) **2 . +XS (I) **2 . +YS (I) **2 . + 

+ ZS (I) **2 . +X**2 . +Y**2 . +Z**2 . -RAML (I) **2 . 

+ +2 . * (X-XS (I) ) * (XM (I) *A (1,1) +YM (I) *A(2 , 1) +ZM (I) *A(3 , 1) ) 

+ +2 . * (Y-YS (I) ) * (XM (I) *A(1, 2) +YM (I) *A(2, 2) +ZM (I) *A(3 , 2) ) 

+ +2 . * (Z-ZS (I) ) * (XM (I) *A (1,3) +YM (I) *A(2, 3) +ZM (I) *A(3 , 3) ) 

+ -2 . * (X*XS (I) +Y*YS (I) +Z*ZS (I) ) 

PFX (I) =2 . * (X+XM (I) *A (1,1) +YM (I) *A(2 , 1) +ZM (I) *A(3 , 1) -XS (I) ) 

PFY (I) =2 . * (Y+XM (I) *A (1,2) +YM ( I ) *A(2, 2) +ZM (I) *A(3 , 2) -YS (I) ) 

PFZ (I) =2 . * (Z+XM (I) *A (1,3) +YM (I) *A(2, 3) +ZM (I) *A (3 , 3 ) -ZS ( I ) ) 

PFS (I) =-2 . * (X-XS (I) ) * (XM (I) *A(1, 2) +YM (I) *A(2, 2) +ZM (I) *A(3 , 2) ) 

+ +2 . * (Y-YS (I) ) * (XM (I) *A (1,1) +YM (I) *A(2 , 1) +ZM (I) *A(3 , 1) ) 

PFT (I) = 2 . * (X-XS (I) ) * (-XM (I) *SIN (T) *COS (S) +YM (I) *SIN (P) *COS (T) * 
+ COS (S) +ZM (I) *COS (P) *COS (T) *COS (S) ) +2 . * (Y-YS (I) ) * (-XM (I) * 

+ SIN (T) *SIN (S) +YM (I) *SIN (P) *COS (T) *SIN (S) +ZM (I) *COS (P) *COS (T) 

+ *SIN ( S ) ) -2 .* (Z-ZS (I) ) * (XM (I) *COS (T) +YM (I) *SIN (P) *SIN (T) 

+ +ZM (I) *COS (P) * SIN (T) ) 

PFP (I) =2 . * (X-XS (I) ) * (YM (I) *A(3 , 1) -ZM (I) *A(2, 1) ) 

+ +2 . * (Y-YS (I) ) * (YM (I) *A(3 , 2) -ZM (I) *A(2, 2) ) 

+ +2 . * (Z-ZS (I) ) * (YM (I) *A(3 , 3) -ZM (I) *A(2, 3) ) 

17 CONTINUE 

DO 1 N=l, 6 

ZEBRA (N) =PFX (N) 

ZEBRA (N+6 ) =PFY (N) 

ZEBRA (N+12 ) =PFZ (N) 

ZEBRA (N+18 ) =PFS (N) 

ZEBRA (N+24 ) =PFT (N) 

ZEBRA (N+3 0 ) =PFP (N) 

1 CONTINUE 
N=6 

CALL SIMQ (ZEBRA, F,N, KS) 

IF(KS.EQ.l) THEN 

WRITE (*,*) ' 1 MATRIX IS SINGULAR' 

GOTO 22 
END IF 
IT=IT+1 

IF ( IT . EQ .51) GO TO 22 
X=X-F (1) 

Y=Y-F (2) 

Z = Z - F (3) 

S=S - F (4) 

T=T-F (5) 

P=P-F (6) 

ZLIM1=0 . 01 
ZLIM2=0 . 1/57 .296 

IF (MAX (ABS (F (1) ) , ABS (F (2) ) , ABS (F (3) ) ) . GT . ZLIM1) GO TO 9 
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GO TO 9 


IF (MAX (ABS (F (4) ) ,ABS(F(5) ) ,ABS(F(6) ) ) .GT.ZLIM2) 
22 SSI(1)=X 
SSI (2) =Y 
SSI (3) =Z 
BETAS (1) =P 
BETAS (2) =T 
BETAS (3) =S 
RETURN 
END 
C 
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8.6. jackdrvr.f 


C COMPUTE THE ACTUATOR EXTENSION COMMANDS BASED ON POSITION 
C IN INERTIAL FRAME. 

C 

SUBROUTINE JACKDRVR 
INCLUDE 'matrixlc.com' 

INCLUDE 'comint2.com' 

INCLUDE 'optint3.com' 

REAL DUMMY3 1A ( 3 , 1) , LLIS (3,3) , LI ( 6 ) , L2 ( 6 ) ,L3 (6) , LENGTHTOT (6) 
REAL LLIMH , LLIML 
C 

q * ********* Langley VMS motion system geometry ********** 

DATA LENEUT/3 . 2649/ 


DATA 

RRS/O . 0254, -0 . 635, 

-2 . 1946/ 



DATA 

AAIS/2 . 1117179, 

0.0762, 0.0, 



X 

2 . 1117179, 

-0.0762, 0.0, 



X 

-0 . 98986594, 

-1.8669, 0.0, 



X 

-1 . 12184942, 

-1.7907, 0.0, 



X 

-1 . 12184942, 

1.7907, 0.0, 



X 

-0 . 98986594, 

1.8669, 0.0/ 



DATA 

BBI I / 1.5021179, 

1 . 9812, 

2 . 

58064, 

X 

1 . 5021179, 

-1 . 9812, 

2 

. 58064, 

X 

0 . 96471232 

, -2.29147116, 

2 

. 58064, 

X 

-2.46682768 

, -0.31027116, 

2 

.58064, 

X 

-2.46682768 

, 0.31027116, 

2 

.58064, 

X 

0 . 96471232 

, 2.29147116, 

2 

. 58064/ 

DATA 

RLI / 6*0./, SSI /3 * 0 

.0/, BETAS/ 3*0 

. 0 

/ 


Q ★ "k "k "k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k -k 

c 

C McFadden Actuator Stroke Limit 

C (When Used Replace LLIMH/LLIML with LLIM) 

C DATA LLIM/O . 92075/ 

C 

C Langley VMS Actuator Stroke Limits 

DATA LLIMH/O. 7864/, LLIML/O. 6487/ 

C 

DATA ACMAX / 0 . 7 / 

DATA FLAG / 6 * 0 / , FLAG2 / 0 / , IT2/ 400/ , NC2/ 400/ , SUMFLAG/ 0/ 
DATA RLID/ 6*0 . /,RLIDD/6*0 . /,RLIO/6*0 . /,RLIDO/6*0 . / 

C 

C EXACT ANGLE COMPUTATIONS 

C 


SINPHI 

= SIN (PHI ) 

SINTH 

= SIN (THE) 

SINPSI 

= SIN(PSI) 

COSPHI 

= COS (PHI) 

COSTH 

= COS (THE) 

COSPSI 

= COS(PSI) 
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c 

c 

c 


FORM LLIS TRANSFORMATION MATRIX 


C 

C 

C 


LLIS (1, 1) 
LLIS (2, 1) 
LLIS (3,1) 
LLIS (1,2) 
LLIS (2,2) 
LLIS (3,2) 
LLIS (1, 3) 
LLIS (2,3) 
LLIS (3,3) 


COS PS I* COS TH 
SINPSI*COSTH 
-SINTH 

COSPSI*SINTH*SINPHI 

SINPSI*SINTH*SINPHI 

COSTH*SINPHI 

COSPSI*SINTH*COSPHI 

SINPSI*SINTH*COSPHI 

COSTH*COSPHI 


- SINPSI*COSPHI 
+ COS PS I* COS PH I 

+ SINPSI*SINPHI 

- COSPSI*SINPHI 


Compute Leg Extensions 
DO JACK = 1,6 

CALL VMULT (LLIS, AAIS (1, JACK) , DUMMY3 1A, 3,3,1) 

LI (JACK) = DUMMY3 1A (1,1) + X - BBII(1,JACK) 

L2 (JACK) = DUMMY31A (2,1) + Y - BBII(2,JACK) 

L3 (JACK) = DUMMY31A (3,1) + Z - BBII(3,JACK) 

LENGTHTOT (JACK) = SQRT (LI ( JACK) **2+L2 ( JACK) **2+L3 ( JACK) **2 ) 
LI (JACK) =LENGTHTOT (JACK) - LENEUT 
END DO 


C 

q * ***************** JACK EXTENSION LIMITING *********************** 
DO JACK=1 , 6 

IF (FLAG ( JACK) .EQ.l) GOTO 5 
C 

C Avail. Length for Same + /- Extension Limits 

C LAVAIL=LLIM-RLI (JACK) *SIGN(1. ,RLID(JACK) ) 

C 

C Avail. Length for Different + /- Extension Limits 

C 

IF (RLI (JACK) .GT. 0 . ) THEN 

LAVAIL=LLIMH-RLI (JACK) *SIGN(1. ,RLID(JACK) ) 

ELSE 

LAVAI L= LL I ML - RL I (JACK) *SIGN(1. ,RLID(JACK) ) 

END IF 
C 

BRAKE (JACK) =ABS (RLID (JACK) ) **2-1 . 98*ACMAX*LAVAIL 
IF (BRAKE (JACK) . LT . 0 . ) GOTO 5 
FLAG (JACK) =1 
VLEAD=RLID (JACK) 

FLAG2=1 
DO JK=1 , 6 

IF (FLAG (JK) . EQ.O) THEN 

RATIO ( JK) =ABS (RLID ( JK) /VLEAD) 

ELSE 

RATIO (JK) =1 . 

END IF 
END DO 

5 END DO 

SUMFLAG=FLAG ( 1 ) +FLAG (2 ) +FLAG (3 ) +FLAG (4 ) +FLAG (5) +FLAG ( 6 ) 

C 

C 
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C When brake is set, determine if brake should be released 

C 

DXX=ABS (X) -ABS (SSI (1) ) 

DYY=ABS (Y) -ABS (SSI (2) ) 

DZZ=ABS (Z) -ABS (SSI (3) ) 

DPHI=ABS (PHI) -ABS (BETAS (1) ) 

DTHE=ABS (THE) -ABS (BETAS (2) ) 

DPSI=ABS (PSI) -ABS (BETAS (3) ) 

DMAX=MAX (DXX, DYY, DZZ , DPHI , DTHE , DPS I ) 

DMIN=MIN (DXX, DYY, DZZ , DPHI , DTHE , DPSI ) 

IF ( (DMAX.LE.l.E-5) .AND. (DMIN . LE . - 0 . 0 1 ) ) THEN 
ID=0 
ELSE 

ID=1 
END IF 

DO JACK=1 , 6 

IF (FLAG ( JACK) . EQ . 0 ) GOTO 20 

RLIDD (JACK) =-ACMAX*SIGN (1 . , RLID (JACK) ) 

RLID (JACK) = RLID (JACK) +RLIDD (JACK) *H 
GOTO 30 

20 IF (SUMFLAG.EQ. 0) GOTO 50 

IF (ABS (RLID (JACK) ) . GT . 0 . 001) THEN 

RLIDD (JACK) =-ACMAX*SIGN (1 . , RLID (JACK) ) * RATIO (JACK) 
RLID (JACK) = RLID (JACK) +RLIDD (JACK) *H 
END IF 

30 IF (ABS (RLID (JACK) ) . GT . (ACMAX*H) ) THEN 

RLI (JACK) =RLI (JACK) +RLID (JACK) *H 

ELSE 

RLID (JACK) =0 . 

FLAG (JACK) =0 
END IF 
GOTO 70 

50 IF (FLAG2 . EQ . 0) GOTO 60 

IF(ID.NE.O) GOTO 70 
FLAG2=0 
IT2 = 0 

60 RLI (JACK) =RLI (JACK) + 

+ (1 . -COS (3 . 1416/2 . *IT2/NC2) ) * (LI (JACK) -RLI (JACK) ) 

IF ( (JACK . EQ . 6 ) .AND. (IT2 .LT.NC2) ) IT2=IT2+1 

C 

70 END DO 

C 

DO JACK=1 , 6 
C 

C Same + /- Extension limits 

C RLI (JACK) =MIN ( 0 . 999*LLIM, MAX ( - 0 . 999*LLIM, RLI (JACK) ) ) 

C 

C Different +/- Extension Limits 

RLI (JACK) =MIN ( 0 . 999*LLIMH , MAX ( - 0 . 999*LLIML , RLI (JACK) ) ) 
C 

RLID (JACK) = (RLI (JACK) -RLIO (JACK) ) /H 
RLIDD (JACK) = (RLID (JACK) -RLIDO (JACK) ) /H 
C 

RLIO (JACK) =RLI (JACK) 

RLIDO (JACK) =RLID (JACK) 

END DO 
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RETURN 

END 
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8.7. liba.f 


c 

c 

c 

c 


COMPUTE THE TRANSFORMATION MATRICES 
SUBROUTINE LIBA 


INCLUDE ' optint3 . com' 

REAL LI A (3, 3) , TA(3,3) 

DATA TA/ 1. ,3*0. 0,1. ,3*0.0,!./ 


C 

C 

C 


C 

C 

C 


C 

C 

C 


C 

C 

C 

C 

C 

C 


EXACT ANGLE COMPUTATIONS 


SINPHI 

SINTH 

SINPSI 

COSPHI 

COSTH 

COSPSI 

TANTH 


SIN (BETAA(l) ) 
SIN (BETAA (2 ) ) 
SIN (BETAA ( 3 ) ) 
COS (BETAA (1) ) 
COS (BETAA (2) ) 
COS (BETAA (3) ) 
SINTH/ COSTH 


FORM LIA TRANSFORMATION MATRIX 


LIA ( 1 , 1) 
LIA ( 2 , 1) 
LIA ( 3 , 1) 
LIA (1,2) 
LIA (2,2) 
LIA (3,2) 
LIA (1,3) 
LIA (2,3) 
LIA (3,3) 


COS PS I* COSTH 
SINPSI* COSTH 
-SINTH 

COSPSI *SINTH*SINPH I - 
SINPSI *SINTH*SINPH I + 
COSTH* SINPHI 
COSPSI*SINTH*COSPHI + 
SINPSI*SINTH*COSPHI - 
COSTH* COS PH I 


FORM TA TRANSFORMATION MATRIX 


TA (1,2) = SINPHI* TANTH 
TA (1,3) =COSPHI *TANTH 
TA (2,2) =COSPHI 
TA (2,3) =-SINPHI 
TA (3 , 2 ) =SINPHI/ COSTH 
TA (3,3) =COSPHI / COSTH 

Compute Inertial Accleration A2 

CALL VMULT (LIA, ACA, A2 ,3,3,1) 


Compute euler Rates BETAAD 
CALL VMULT (TA,WAA, BETAAD, 3, 3,1) 


RETURN 

END 


LIA AND TS 


SINPSI* COS PH I 
COS PS I* COS PH I 

SINPSI *SINPH I 
COSPSI *SINPH I 
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8.8. newopt4.f 

C***** SUBROUTINE NEWOPT4 . F ****** 

C 

C NONLINEAR WASHOUT ALGORITHM: ANG. VELOCITY DEVELOPMENT. 

C GIVEN A/C ACCELS ACA AND EULER RATES BETAAD, 

C COMPUTE SIMULATOR INERTIAL DISPLACEMENT AND EULER ANGLES. 

C 

SUBROUTINE NEWOPT4 (MODE) 

INCLUDE 'comint2.com' 

INCLUDE 'wcom2.com' 

INCLUDE 'optint3.com' 

INCLUDE 'nopt4.com' 

INCLUDE 'matrixlc.com' 

C DATA I RE SET/ 0/ , I HOLD/ 0/ 

C 

C Compute Fairing Parameters 

C 

SQWASH=SQWASHP*EA+SQWASHI * (1 . O-EA) 

DELSQ=MAX (MIN ( SQWASH- SQWASHP , 1.0) ,-1.0) 

SQWASHP=SQWASH+DELSQ 

A=1 . 0- SQWASHP 
AA=1 . O-SQWASHI 
C 

C Fairing of Heave Nonlinear Gain and Limit 

C 

GZ4 (1) =AA*GZ4 0 (1) +SQWASHI*GZ4S (1) 

GZ4 (2) =AA*GZ4 0 (2) +SQWASHI*GZ4S (2) 

GZ4 (3) =AA*GZ4 0 (3) +SQWASHI*GZ4S (3) 

AMX4 =AA* AMX4 0 + SQWASHI * AMX4 S 
C 

C Fairing of Runway Roughness Amplitude 

C 

XKA=A*XKAO+ SQWASHP *XKAS 

IF (MODE . EQ . 1 ) THEN 
H = DT 
C 

C Set "old" variables for future use in HOLD and OPERATE modes 

C 

DO 1=1,3 

A2N0 (I) =0 . 

BADNO (I) =0 . 

END DO 

DO 1=1,9 

XXO (I) =0 . 

XYO (I) =0 . 
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END DO 


DO J=l, 11 

DO 1=1,11 

IF (I.GE.J) THEN 

PX (I , J) =PXVEC ( (J-l) *11- J* (J-l) /2+I) 

PX (J, I) =PX(I, J) 

PY (I , J) =PYVEC ( (J-l) *11- J* (J-l) /2+I) 

PY ( J, I ) =PY ( I , J) 

END IF 
END DO 
END DO 

DO 1=1,4 

XRO (I) =0 . 

END DO 

DO J=1 , 5 
DO 1=1,5 

IF (I.GE.J) THEN 

PR (I , J) =PRVEC ( (J-l) *5- J* (J-l) /2+I) 

PR (J, I) =PR (I , J) 

END IF 
END DO 
END DO 

DO 1=1,5 

XZO (I) =0 . 

END DO 

DO J=l, 6 
DO 1=1,6 

IF (I.GE.J) THEN 

PZ (I, J) =PZVEC ( ( J-l) *6- J* ( J-l) /2+I) 

PZ (J, I) =PZ (I, J) 

END IF 
END DO 
END DO 

CALL RESETC2 
C 

C Set "old" variables for future use in HOLD and OPERATE modes 

C 

BETASRO (1) =PHI 
BETASRO (2) =THE 
BSDRO (1) =PHID 
BSDRO (2) =THED 

DO 1=1,2 

XHO (I) =0 . 

ACAO (I) =0 . 

BSDTO (I) =0 . 

BETASTO (I) =0 . 

BETASTLO (I) =0 . 

XTO=0 . 

XT2O=0 . 

WGUSTO=0 . 
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ACZT=0 . 

END DO 

GO TO 1 
END IF 

IF (MODE. EQ. 2) THEN 
CALL WTRIM3 
END IF 
C 

C Compute Augmented Acceleration from W-Gust 

C 

IF (MODE. EQ. 3) THEN 

WGAV=0 . 5* (WGUST+WGUSTO) 

XT=XTO+DT* ( -G2D1*XT0+XT20+ (G2N1-G2D1*G2N2 ) *WGAV) 
XTAV=0 .5* (XT+XTO) 

XT2=XT20+DT* (-G2D0*XTAV+ (G2N0 -G2D0 *G2N2 ) *WGAV) 
ACZT=XT+G2N2 * WGUST 
XTO=XT 
XT20=XT2 
WGUS TO = WGUST 
C 

C (first-order turbulence model no longer used) 

C 

C XT=XTO+DT* ( -G1D0 *XTO+ (G1N0 -G1D0 *G1N1 ) *WGUSTO) 

C XTO=XT 

C WGUS TO = WGUST 

C ACZT=XT+G1N1* WGUST 

END IF 

CALL LI BA 
CALL GAINOPT4 

A2N ( 3 ) =A2N ( 3 ) +GT4 *ACZT 

IF (MODE. EQ. 3) THEN 
CALL NFILX 
CALL NFILY 
CALL NFILZ 
CALL NFILR 
CALL STATE4 

ELSE IF (MODE. EQ. 2) THEN 
CALL STATE4 
CALL STATE4 
CALL STATE4 
CALL STATE4 

END IF 


IF (MODE. EQ. 3) CALL INTEG4 

1 RETURN 
END 
C 
C 
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8.9. nfilr.f 


c 

C Yaw Filter Neurocomputing Solver for the Riccati Equation 

C 

SUBROUTINE NFILR 

INCLUDE ' comint2 . com ' 

INCLUDE 'nopt4.com' 

REAL SPR (5, 5) , SUMER, SUMPR, SUMPRT 

REAL PAPR (5,5), UPBR (5,5), UAPR (5,5), APUR (5,5) 

REAL EUR ( 5 , 5 ) , UR ( 5 ) , PZR ( 5 ) 

C 

C Compute Prescribed Nonlinearity Alpha 

C 

ALPR=PSI*Q2R*PSI 

IF (ALPR.GE . ALPRMAX) ALPR=ALPRMAX 
DO 1=1,5 

APR (1,1) =APRO ( I ) +ALPR 
END DO 
C 

C Start Training Iterations & Initialize Variables 

C 

DO L=1 , 3 

DO 1=1,5 
DO J=l, 5 

SPR ( I , J) =0 . 

UPBR (I , J) =0 . 

PAPR (I , J) =0 . 

UAPR (I , J) =0 . 

APUR (I , J) =0 . 

END DO 
END DO 
C 

C Compute Matrix Products SP and PA 

C 

DO 1=1,5 
DO J=l, 5 

DO K=1 , 5 

SPR (I, J) =SPR ( I , J) +BRBR ( I , K) *PR (K, J) 

END DO 
END DO 
DO K=l, 5 

PAPR (1 , 1) = PAPR ( 1 , 1) +PR (I , K) *APR (K, 1) 

PAPR (I , 3) = PAPR (I , 3) +PR (I , K) *APR (K, 3) 

PAPR (I , 5) = PAPR (I , 5) +PR (I , K) *APR (K, 5) 

END DO 

PAPR (I , 2) =PR (I , 1) * APR (1, 2) +PR (I , 2) *APR (2, 2) 

PAPR (I, 4) =PR (1,4) *APR (4,4) 

END DO 
C 

C Compute Error Signal v. Vector p and Matrix Product pz 

C Note: Matrix Product A'P is transpose of PA by symmetry of P 
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c 


DO 1=1,5 
UR (I) =0 . 

PZR (I) =0 . 

DO J=l, 5 

IF(I.LE.J) THEN 
SUMER=0 . 

DO K=1 , 5 

SUMER=SUMER+PR ( I , K) *SPR(K, J) 

END DO 

EUR (I, J) = SUMER -PAPR (I , J) -PAPR(J, I) -R1PR ( I , J) 
EUR ( J, I ) =EUR ( I , J) 

END IF 

UR ( I ) =UR ( I ) +EUR ( I , J) *ZR (L, J) 

PZR (I) =PZR ( I ) +PR ( I , J) *ZR(L, J) 

END DO 
END DO 
C 

C Compute Matrix Product Avz 

C 

DO 1=1,2 

DO J=l, 5 

DO K=1 , 3 

APUR(I, J) =APUR ( I , J) +APR ( I , K) *UR(K) *ZR(L, J) 
END DO 

APUR (I, J) =APUR (I , J) +APR (1,5) *UR (5) *ZR (L, J) 

END DO 
END DO 
DO 1=3,4 

DO J=l, 5 

APUR (I, J) =APUR ( I , J) +APR ( I , 1) *UR(1) *ZR (L , J) 

DO K=3 , 5 

APUR (I, J) =APUR ( I , J) +APR ( I , K) *UR(K) *ZR (L , J) 
END DO 
END DO 
END DO 

DO J=l, 5 

APUR (5, J) =APR (5,5) *UR(5) *ZR(L, J) 

END DO 
C 

C Compute Matrix Product vzA 

C 

DO 1=1,5 

DO K=1 , 5 

UAPR (I , 1) =UAPR (I , 1) +UR (I) *ZR (L, K) *APR (K, 1) 

UAPR (1,3) =UAPR (1,3) +UR (I) *ZR (L, K) *APR (K, 3) 

UAPR (I , 5) =UAPR (1,5) +UR (I) *ZR (L, K) *APR (K, 5) 

END DO 

UAPR (I, 2) =UR (I) *ZR (L, 1) * APR (1, 2) 
x +UR ( I ) * ZR ( L , 2 ) * APR (2,2) 

UAPR (I, 4) =UR ( I ) *ZR (L , 4 ) *APR(4,4) 

END DO 
C 

C Compute Matrix Product vp ' S 

C 

DO 1=1,5 

DO J=1 , 4 
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DO K=1 , 4 

UPBR ( I , J) =UPBR ( I , J) +UR ( I ) *PZR (K) *BRBR (K, J) 

END DO 
END DO 
END DO 
C 

C Sum Avz + vzA + vp ' S and integrate to update Riccati Solution P 

C 

DO 1=1,5 

DO J=l, 5 

IF(I.LE.J) THEN 

SUMPR=APUR ( I , J) +UAPR ( I , J) -UPBR ( I , J) 

IF(I.NE.J) THEN 

SUMPRT=APUR (J, I) +UAPR (J, I) -UPBR (J, I) 

ELSE 

SUMPRT=SUMPR 
END IF 

PR (I , J) =PR (I , J) +MUR* ( SUMPR+ SUMPRT ) 

PR (J, I) =PR (I , J) 

END IF 
END DO 
END DO 

END DO 

RETURN 

END 
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8.10. nfilx.f 


c 

C Surge Filter Neurocomputing Solver for the Riccati Equation 

C 

SUBROUTINE NFILX 
INCLUDE ' comint2 . com ' 

INCLUDE 'nopt4.com' 

REAL SPX (11, 11) , SUMEX, SUMPX, SUMPXT 

REAL PAPX (11, 11) ,UPBX (11, 11) , UAPX (11, 11) ,APUX (11, 11) 

REAL EUX (11, 11) ,UX (11) , PZX (11) 


C 

C Compute Prescribed Nonlinearity Alpha 

C 

ALPX=X*Q2X (1) *X+XD*Q2X (2) *XD 
IF (ALPX . GT . ALPXMAX) ALPX=ALPXMAX 
DO 1=1,11 

APX (1,1) =APXO ( I ) +ALPX 
END DO 
C 

C Start Training Iterations & Initialize Variables 

C 

DO L=1 , 3 

DO 1=1,11 

DO J=l, 11 

SPX (I , J) =0 . 0 
PAPX (I , J) =0 . 0 
UPBX (I , J) =0 . 0 
UAPX (I , J) =0 . 0 
APUX (I , J) =0 . 0 
END DO 
END DO 
C 

C Compute Matrix Product SP 

C 

DO 1=1,6 

DO J=l, 11 
DO K=1 , 6 

SPX (I, J) =SPX (I, J) +BRBX (I , K) *PX (K, J) 
END DO 

SPX (I, J) =SPX (I, J) +BRBX (I, 9) *PX (9, J) 

END DO 
END DO 

DO J=l, 11 
DO K=1 , 6 

SPX (9, J) =SPX (9, J) +BRBX (9, K) *PX (K, J) 
END DO 

SPX (9, J) =SPX (9, J) +BRBX (9, 9) *PX (9, J) 

END DO 
C 

C Compute Matrix Product PA 
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c 


DO 1=1,11 
DO J=l, 6 

DO K=l, 6 

PAPX (I, J) =PAPX (I , J) +PX (I, K) *APX (K, J) 

END DO 
END DO 

PAPX (1,7) =PX (1,7) *APX (7,7) 

PAPX (1,8) =PX (1,7) *APX (7,8) +PX (1,8) *APX (8,8) 

PAPX (1,9) =PX (1,8) *APX (8,9) +PX (1,9) *APX (9,9) 

DO K=l, 6 

PAPX (I, 10) =PAPX (I , 10) +PX (I, K) *APX (K, 10) 

END DO 

PAPX (I, 10) =PAPX (1 , 10) +PX (1 , 10) *APX (10 , 10) 

DO K=l, 6 

PAPX (I , 11) =PAPX (I , 11) +PX (I , K) *APX (K, 11) 

END DO 

PAPX (I , 11) =PAPX (I , 11) +PX (I , 11) *APX (11, 11) 

END DO 
C 

C Compute Error Signal v. Vector p and Matrix Product vz 

C Note: Matrix Product A'P is transpose of PA by symmetry of P 

C 

DO 1=1,11 
UX (I) =0 . 

PZX (I) =0 . 

DO J=l, 11 

IF(I.LE.J) THEN 
SUMEX=0 . 

DO K=l, 11 

SUMEX=SUMEX+PX (I , K) *SPX (K, J) 

END DO 

EUX ( I , J ) = SUMEX - PAPX ( J , I ) -PAPX (I, J) -R1PX ( I , J) 

EUX (J, I) =EUX (I, J) 

END IF 

UX (I) =UX (I) +EUX (I, J) *ZX (L, J) 

PZX (I) =PZX (I) +PX (I, J) *ZX (L, J) 

END DO 
END DO 
C 

C Compute Matrix Product Avz 

C 

DO 1=1,6 

DO J=l, 11 
DO K=1 , 6 

APUX (I, J) =APUX (I , J) +APX (I , K) *UX (K) *ZX (L, J) 

END DO 

APUX (I, J) =APUX (I , J) +APX (1 , 10) *UX (10) *ZX (L, J) 
x +APX (I, 11) *UX (11) *ZX (L, J) 

END DO 
END DO 

DO J=l, 11 

APUX (7, J) =APX (7, 7) *UX (7) *ZX (L, J) 
x +APX (7, 8) *UX (8) *ZX (L, J) 

APUX (8, J) =APX (8, 8) *UX (8) *ZX (L, J) 
x +APX (8, 9) *UX (9) *ZX (L, J) 

APUX (9, J) =APX (9, 9) *UX (9) *ZX (L, J) 
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APUX (10, J) =APX (10 ,10) *UX (10) *ZX (L, J) 

APUX (11, J) =APX (11, 11) *UX (11) *ZX (L, J) 

END DO 
C 

C Compute Matrix Product vzA 

C 

DO 1=1,11 
DO J=l, 6 

DO K=l, 6 

UAPX (I, J) =UAPX (I, J) +UX (I) *ZX (L, K) *APX (K, J) 

END DO 
END DO 

UAPX (I , 7) =UX (I) *ZX (L, 7) *APX (7, 7) 

UAPX (I, 8) =UX (I) *ZX (L, 7) *APX (7, 8) 
x +UX (I) *ZX (L, 8) *APX (8, 8) 

UAPX (I , 9) =UX (I) *ZX (L, 8) *APX (8, 9) 
x +UX (I) *ZX (L, 9) *APX (9, 9) 

DO K=l, 6 

UAPX (I , 10) =UAPX (I , 10) +UX (I) *ZX (L, K) *APX (K, 10) 

END DO 

UAPX (1 , 10) =UAPX (1 , 10) +UX (I) *ZX (L, 10) *APX (10, 10) 

DO K=l, 6 

UAPX (I , 11) =UAPX (I , 11) +UX (I) *ZX (L, K) *APX (K, 11) 

END DO 

UAPX (I , 11) =UAPX (I , 11) +UX (I) *ZX (L, 11) *APX (11, 11) 

END DO 
C 

C Compute Matrix Product vp ' S 

C 

DO 1=1,11 
DO J=l, 6 

DO K=l, 6 

UPBX (I , J) =UPBX (I , J) +UX (I) *PZX (K) *BRBX (K, J) 

END DO 

UPBX (I , J) =UPBX (I , J) +UX (I) *PZX (9) *BRBX (9, J) 

END DO 

DO K=l, 6 

UPBX (1 , 9) =UPBX (1 , 9) +UX (I) *PZX (K) *BRBX (K, 9) 

END DO 

UPBX (I , 9) =UPBX (I , 9) +UX (I) *PZX (9) *BRBX (9, 9) 

END DO 
C 

C Sum Avz + vzA + vp ' S and integrate to update Riccati Solution P 

C 

DO 1=1,11 

DO J=l, 11 

IF(I.LE.J) THEN 

SUMPX=APUX ( I , J) +UAPX ( I , J) -UPBX ( I , J) 

IF(I.NE.J) THEN 

SUMPXT=APUX (J, I) +UAPX (J, I) -UPBX (J, I) 

ELSE 

SUMPXT=SUMPX 
END IF 

PX (I , J) =PX (I , J) +MUX* (SUMPX+SUMPXT) 

PX (J, I) =PX (I, J) 

END IF 
END DO 
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END DO 
END DO 

RETURN 

END 
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8.11. nfily.f 


c 

C Sway Filter Neurocomputing Solver for the Riccati Equation 

C 

SUBROUTINE NFILY 
INCLUDE ' comint2 . com ' 

INCLUDE 'nopt4.com' 

REAL SPY (11, 11) , SUMEY, SUMPY, SUMPYT 

REAL PAPY (11, 11) , UPBY ( 11 , 11) ,UAPY(11, 11) ,APUY(11, 11) 

REAL EUY (11,11) , UY (11), PZY (11) 


C 

C Compute Prescribed Nonlinearity Alpha 

C 

ALPY=Y*Q2Y (1) *Y+YD*Q2Y (2) *YD 
IF (ALPY . GT . ALPYMAX) AL P Y = AL P YMAX 
DO 1=1,11 

APY (1,1) =APYO ( I ) +ALPY 
END DO 
C 

C Start Training Iterations & Initialize Variables 

C 

DO L=1 , 3 

DO 1=1,11 

DO J=l, 11 

SPY (I , J) =0 . 0 
PAPY (I , J) =0 . 0 
UPBY (I , J) =0 . 0 
UAPY (I , J) =0 . 0 
APUY (I , J) =0 . 0 
END DO 
END DO 
C 

C Compute Matrix Product SP 

C 

DO 1=1,6 

DO J=l, 11 
DO K=1 , 6 

SPY (I, J) =SPY ( I , J) +BRBY ( I , K) *PY (K, J) 
END DO 

SPY (I, J) =SPY ( I , J) +BRBY (1,9) *PY ( 9 , J) 

END DO 
END DO 

DO J=l, 11 
DO K=1 , 6 

SPY (9, J) =SPY ( 9 , J) +BRBY ( 9 , K) *PY (K, J) 
END DO 

SPY (9, J) =SPY (9, J) +BRBY (9, 9) *PY (9, J) 

END DO 
C 

C Compute Matrix Product PA 
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c 


c 

c 

c 

c 


DO 1=1,11 
DO J=l, 6 

DO K=l, 6 

PAPY ( I , J ) =PAPY ( I , J) +PY ( I , K) *APY (K, J) 

END DO 
END DO 

PAPY (1,7) =PY (1,7) *APY (7,7) 

PAPY (1,8) =PY (1,7) *APY ( 7 , 8 ) +PY ( I , 8 ) *APY (8,8) 

PAPY (1,9) =PY (1,8) *APY ( 8 , 9 ) +PY ( I , 9 ) *APY (9,9) 

DO K=l, 6 

PAPY (I, 10) = PAPY (I, 10) +PY ( I , K) *APY(K, 10) 

END DO 

PAPY (I, 10) = PAPY (I, 10) +PY ( 1 , 10) *APY ( 10 , 10) 

DO K=l, 6 

PAPY (1 , 11) = PAPY ( 1 , 11) +PY (I , K) *APY (K, 11) 

END DO 

PAPY (I, 11) = PAPY (I, 11) +PY ( I , 11) *APY ( 11 , 11) 

END DO 

Compute Error Signal v. Vector p and Matrix Product vz 
Note: Matrix Product A'P is transpose of PA by symmetry of P 


DO 1=1,11 
UY (I) =0 . 

PZY (I) =0 . 

DO J=l, 11 

IF(I.LE.J) THEN 
SUMEY=0 . 

DO K=l, 11 

SUMEY=SUMEY+PY (I , K) *SPY (K, J) 

END DO 

EUY ( I , J ) =SUMEY-PAPY (J, I) -PAPY (I, J) -R1PY ( I , J) 
EUY ( J, I ) =EUY ( I , J) 

END IF 

UY ( I ) =UY ( I ) +EUY ( I , J) *ZY (L, J) 

PZY (I) =PZY(I) +PY ( I , J) *ZY (L , J) 

END DO 
END DO 


C 

C Compute Matrix Product Avz 

C 

DO 1=1,6 

DO J=l, 11 
DO K=l, 6 

APUY ( I , J ) = APUY ( I , J ) +APY ( I , K) *UY(K) *ZY (L , J) 
END DO 

APUY (I, J) = APUY ( I , J ) +APY(I, 10) *UY(10) *ZY (L, J) 
x +APY (I , 11) *UY (11) *ZY (L, J) 

END DO 
END DO 

DO J=l, 11 

APUY (7, J) =APY ( 7 , 7) *UY(7) *ZY (L, J) 
x +APY (7,8) *UY ( 8 ) * ZY ( L , J ) 

APUY (8, J) =APY (8,8) *UY(8) *ZY (L, J) 
x +APY (8,9) *UY ( 9 ) * ZY ( L , J ) 
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APUY (9, J) =APY (9, 9) *UY (9) *ZY (L, J) 

APUY ( 1 0 , J ) = APUY ( 1 0 , J ) +APY(10, 10) *UY(10) *ZY (L, J) 

APUY (11, J) = APUY ( 1 1 , J ) +APY ( 11 , 11) *UY(11) *ZY (L, J) 

END DO 
C 

C Compute Matrix Product vzA 

C 

DO 1=1,11 
DO J=l, 6 

DO K=l, 6 

UAPY ( I , J ) =UAPY ( I , J) +UY ( I ) *ZY (L , K) *APY (K, J) 

END DO 
END DO 

UAPY (I, 7) =UY ( I ) *ZY(L, 7) *APY(7, 7) 

UAPY (I, 8) =UY ( I ) *ZY(L, 7) *APY(7, 8) 
x +UY ( I ) * ZY ( L , 8 ) * APY (8,8) 

UAPY (I , 9) =UY (I) *ZY(L, 8) *APY(8, 9) 
x +UY (I) *ZY (L, 9) *APY (9, 9) 

DO K=l, 6 

UAPY (I , 10) =UAPY (I , 10) +UY (I) *ZY (L, K) *APY (K, 10) 

END DO 

UAPY (I, 10) =UAPY ( I , 10) +UY ( I ) *ZY(L, 10) *APY(10, 10) 

DO K=l, 6 

UAPY (I, 11) =UAPY ( I , 11) +UY ( I ) *ZY(L,K) *APY(K, 11) 

END DO 

UAPY (I, 11) =UAPY ( I , 11) +UY ( I ) *ZY(L, 11) *APY(11, 11) 

END DO 
C 

C Compute Matrix Product vp ' S 

C 

DO 1=1,11 
DO J=l, 6 

DO K=l, 6 

UPBY ( I , J) =UPBY ( I , J) +UY ( I ) *PZY (K) *BRBY (K, J) 

END DO 

UPBY (I, J) =UPBY ( I , J) +UY ( I ) *PZY ( 9 ) *BRBY ( 9 , J) 

END DO 

DO K=l, 6 

UPBY (I, 9) =UPBY ( I , 9) +UY(I) *PZY(K) *BRBY (K, 9) 

END DO 

UPBY (I, 9) =UPBY ( I , 9) +UY(I) *PZY(9) *BRBY ( 9 , 9) 

END DO 
C 

C Sum Avz + vzA + vp ' S and integrate to update Riccati Solution P 

C 

DO 1=1,11 

DO J=l, 11 

IF(I.LE.J) THEN 

SUMPY=APUY ( I , J) +UAPY ( I , J) -UPBY ( I , J) 

IF(I.NE.J) THEN 

SUMPYT=APUY (J, I) +UAPY (J, I) -UPBY (J, I) 

ELSE 

SUMPYT=SUMPY 
END IF 

PY ( I , J) =PY(I, J) +MUY* (SUMPY+SUMPYT) 

PY ( J, I ) =PY ( I , J) 

END IF 
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END DO 
END DO 

END DO 

RETURN 

END 
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8.12. nfilz.f 


c 

C Heave Filter Neurocomputing Solver for the Riccati Equation 

C 

SUBROUTINE NFILZ 

INCLUDE ' comint2 . com ' 

INCLUDE 'nopt4.com' 

REAL SPZ (6, 6) , SUMEZ, SUMPZ, SUMPZT 

REAL PAPZ (6,6) , UPBZ (6,6) , UAPZ (6,6) , APUZ (6,6) 

REAL EUZ (6,6) , UZ ( 6 ) , PZZ ( 6 ) 


C 

C Compute Prescribed Nonlinearity Alpha 

C 

ALPZ=Z*Q2Z (1) *Z+ZD*Q2Z (2) *ZD 
IF (ALPZ . GT . ALPZMAX) ALPZ=ALPZMAX 
DO 1=1,6 

APZ (1,1) =APZO ( I ) +ALPZ 
END DO 
C 

C Start Training Iterations & Initialize Variables 

C 

DO L=1 , 3 

DO 1=1,6 
DO J=l, 6 

SPZ (I , J) =0 . 

PAPZ (I , J) =0 . 

UPBZ (I , J) =0 . 

UAPZ (I , J) =0 . 

APUZ (I , J) =0 . 

END DO 
END DO 
C 

C Compute Matrix Product SP 

C 

DO 1=1,2 
DO J=l, 6 

SPZ (I, J) =BRBZ (I, 1) *PZ (1, J) 

x +BRBZ (I , 2) *PZ (2, J) +BRBZ (I , 5 ) *PZ (5 , J) 

END DO 
END DO 

DO J=l, 6 

SPZ (5, J) =BRBZ (5, 1) *PZ (1, J) 

x +BRBZ (5, 2) *PZ (2, J) +BRBZ (5,5) *PZ (5 , J) 

END DO 
C 

C Compute Matrix Product PA 

C 


DO 1=1,6 

PAPZ (1,1) =PZ (1,1) *APZ (1,1) +PZ (1,2) *APZ (2,1) 
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PAPZ (1,2) =PZ (1,1) *APZ (1,2) +PZ (1,2) *APZ (2,2) 

PAPZ (I , 3) =PZ (I , 3) *APZ (3,3) 

PAPZ (1,4) =PZ (1,3) *APZ (3,4) +PZ (1,4) *APZ (4,4) 

PAPZ (1,5) =PZ (1,4) *APZ (4,5) +PZ (1,5) *APZ (5,5) 

PAPZ (1,6) =PZ (1,1) *APZ (1,6) +PZ (1,2) *APZ (2,6) 
x +PZ (I, 6) *APZ (6, 6) 

END DO 
C 

C Compute Error Signal v. Vector p and Matrix Product vz 

C Note: Matrix Product A'P is transpose of PA by symmetry of P 

C 

DO 1=1,6 
UZ (I) =0 . 

PZZ (I) =0 . 

DO J=l, 6 

IF(I.LE.J) THEN 

SUMEZ=0 . 

DO K=l, 6 

SUMEZ = SUMEZ + PZ (I,K) *SPZ (K, J) 

END DO 

EUZ (I, J) =SUMEZ - PAPZ (J, I) -PAPZ (I, J) -R1PZ (I, J) 

EUZ (J, I) =EUZ (I, J) 

END IF 

UZ (I) =UZ (I) +EUZ (I, J) *ZZ (L, J) 

PZZ (I) =PZZ (I) +PZ (I, J) *ZZ (L, J) 

END DO 
END DO 
C 

C Compute Matrix Product Avz 

C 

DO 1=1,2 

DO J=l, 6 

APUZ (I , J) =APZ (1,1) *UZ (1) *ZZ (L, J) 
x +APZ (1,2) *UZ (2) *ZZ (L, J) +APZ (1,6) *UZ (6) *ZZ (L, J) 

END DO 
END DO 

DO J=l, 6 

APUZ (3 , J) =APZ (3,3) *UZ (3) *ZZ (L, J) 
x +APZ (3 , 4) *UZ (4) *ZZ (L, J) 

APUZ (4, J) =APZ (4,4) *UZ (4) *ZZ (L, J) 
x +APZ (4, 5) *UZ (5) *ZZ (L, J) 

APUZ (5, J) =APZ (5, 5) *UZ (5) *ZZ (L, J) 

APUZ (6, J) =APZ (6, 6) *UZ (6) *ZZ (L, J) 

END DO 
C 

C Compute Matrix Product vzA 

C 

DO 1=1,6 

UAPZ (1,1) =UZ (I) *ZZ (L, 1) *APZ (1,1) +UZ (I) *ZZ (L, 2) *APZ (2,1) 
UAPZ (1,2) =UZ (I) *ZZ (L, 1) *APZ (1,2) +UZ (I) *ZZ (L, 2) *APZ (2,2) 
UAPZ (I , 3) =UZ (I) *ZZ (L, 3) *APZ (3,3) 

UAPZ (1,4) =UZ (I) *ZZ (L, 3) *APZ (3,4) +UZ (I) *ZZ (L, 4) *APZ (4,4) 
UAPZ (I, 5) =UZ (I) *ZZ (L, 4) *APZ (4, 5) +UZ (I) *ZZ (L, 5) *APZ (5,5) 
UAPZ (1,6) =UZ (I) *ZZ (L, 1) *APZ (1,6) 
x +UZ (I) *ZZ (L, 2) *APZ (2,6) +UZ (I) *ZZ (L, 6) *APZ (6,6) 
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END DO 
C 

C Compute Matrix Product vp ' S 

C 

DO 1=1,6 

DO J=1 , 2 

UPBZ (I , J) =UZ (I) *PZZ (1) *BRBZ (1, J) 
x +UZ (I) *PZZ (2) *BRBZ (2, J) +UZ (I) *PZZ (5) *BRBZ (5, J) 

END DO 

UPBZ (1,5) =UZ (I) *PZZ (1) *BRBZ (1,5) 
x +UZ (I) *PZZ (2) *BRBZ (2,5) +UZ (I) *PZZ (5) *BRBZ (5,5) 


END DO 
C 

C Sum Avz + vzA + vp ' S and integrate to update Riccati Solution P 

C 

DO 1=1,6 

DO J=l, 6 

IF(I.LE.J) THEN 

SUMPZ=APUZ ( I , J) +UAPZ ( I , J) -UPBZ ( I , J) 

IF(I.NE.J) THEN 

SUMPZT=APUZ (J, I) +UAPZ (J, I) -UPBZ (J, I) 

ELSE 

SUMPZT=SUMPZ 
END IF 

PZ (I , J) =PZ (I , J) +MUZ* (SUMPZ+SUMPZT) 

PZ (J, I) =PZ (I, J) 

END IF 
END DO 
END DO 

END DO 

RETURN 

END 
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8.13. ofil3.f 


C OPTIMAL FILTERS (ALL IN INERTIAL FRAME) : 

C 

C 6TH ORDER PH I AD TO PHISD 

C 6TH ORDER A2(2) TO PHISD 

C 6TH ORDER A2(2) TO AYSI 

C 6TH ORDER THETAAD TO THETASD 

C 6TH ORDER A2(l) TO THETASD 

C 6TH ORDER A2(l) TO AXSI 

C 4TH ORDER PSIA TO PSIS 

C 4TH ORDER A2(3) TO AZSI 

C 

SUBROUTINE OFIL3 

REAL B1 ( 8 ) , Cl ( 8 ) , D1 ( 8 ) , El ( 8 ) , FI ( 6 ) , G1 ( 6 ) 

REAL B2 ( 8 ) , C2 ( 8 ) , D2 ( 8 ) , E2 ( 8 ) , F2 ( 6 ) , G2 ( 6 ) 

INCLUDE 'matrixlc.com' 

INCLUDE 'optint3.com' 

INCLUDE 'wopt3.com' 

C 

q* ********************** * block NO. Cl **************************** 

C FILTER FROM PH I AD TO PHISD: 

C 

B1 (1) =DT* ( -R1D5 *X10 ( 1 ) +X20 (1) + (R1N5 -R1D5 *R1N6 ) *BADNO (1) ) 

Cl (1) =DT* ( -R1D4 *X10 ( 1 ) +X30 (1) + (R1N4 -R1D4 *R1N6 ) *BADNO (1) ) 

D1 (1) =DT* ( -R1D3 *X10 ( 1 ) +X40 (1) + (R1N3 -R1D3 *R1N6 ) *BADNO (1) ) 

El (1) =DT* ( -R1D2 *X10 ( 1 ) +X50 (1) + (R1N2 -R1D2 *R1N6 ) *BADNO (1) ) 

FI (1) =DT* (-R1D1*X10 (1) +X60 (1) + (R1N1-R1D1*R1N6 ) *BADNO (1) ) 

G1 (1) =DT* ( -R1D0 *X10 ( 1 ) + (R1N0 -R1D0 *R1N6 ) *BADNO (1) ) 

B2 (1) =DT* 

x ( -R1D5 * (X10 (1) +B1 (1) ) +X20 (1) +C1 (1) + (R1N5-R1D5*R1N6 ) *BADN (1) ) 
C2 (1) =DT* 

x ( -R1D4 * (X10 (1) +B1 (1) ) +X30 (1) +D1 (1) + (R1N4 -R1D4 *R1N6 ) *BADN (1) ) 
D2 (1) =DT* 

x ( -R1D3 * (X10 (1) +B1 (1) ) +X40 (1) +E1 (1) + (R1N3 -R1D3 *R1N6 ) *BADN (1) ) 
E2 (1) =DT* 

x ( -R1D2 * (X10 (1) +B1 (1) ) +X50 (1) +F1 (1) + (R1N2 -R1D2 *R1N6 ) *BADN (1) ) 
F2 (1) =DT* 

x (-R1D1* (X10 (1) +B1 (1) ) +X60 (1) +G1 (1) + (R1N1-R1D1*R1N6 ) *BADN (1) ) 
G2 (1) =DT* 

x (-R1D0* (X10 (1) +B1 (1) ) + (R1N0 -R1D0 *R1N6 ) *BADN (1) ) 

XXI (1) =X10 (1) +0 . 5* (B1 (1) +B2 (1) ) 

X2 (1) =X20 (1) +0 . 5* (Cl (1) +C2 (1) ) 

X3 (1) =X30 (1) +0 . 5* (D1 (1) +D2 (1) ) 

X4 (1) =X40 (1) +0 . 5* (El (1) +E2 (1) ) 

X5 (1) =X50 (1) +0 . 5* (FI (1) +F2 (1) ) 

X6 (1) =X60 (1) +0 . 5* (G1 (1) +G2 (1) ) 

C 
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q* ********************** * BLOCK NO. C2 **************************** 
C FILTER FROM A2(2) TO PHISD: 

C 

B1 (2) =DT* ( -R1D5 *X10 (2) +X20 (2) + (R2N5 -R1D5 *R2N6 ) *A2NO (2) ) 

Cl (2) =DT* ( -R1D4 *X10 (2) +X30 (2) + (R2N4 -R1D4 *R2N6 ) *A2NO (2) ) 

D1 (2) =DT* ( -R1D3 *X10 (2) +X40 (2) + (R2N3 -R1D3 *R2N6 ) *A2NO (2) ) 

El (2) =DT* ( -R1D2 *X10 (2) +X50 (2) + (R2N2 -R1D2 *P2N6 ) *A2NO (2) ) 

FI (2) =DT* (-R1D1*X10 (2) +X60 (2) + (R2N1-R1D1*P2N6 ) *A2NO (2) ) 

G1 (2) =DT* ( -R1D0 *X10 (2) + (R2N0 -R1D0 *P2N6 ) *A2NO (2) ) 

B2 (2) =DT* 

x ( -R1D5 * (X10 (2) +B1 (2) ) +X20 (2) +C1 (2) + (R2N5-R1D5*R2N6 ) *A2N (2) ) 
C2 (2) =DT* 

x ( -R1D4 * (XIO (2) +B1 (2) ) +X30 (2) +D1 (2) + (R2N4 -R1D4 *R2N6 ) *A2N (2) ) 
D2 (2) =DT* 

x ( -R1D3 * (XIO (2) +B1 (2) ) +X40 (2) +E1 (2) + (R2N3 -R1D3 *R2N6 ) *A2N (2) ) 
E2 (2) =DT* 

x ( -R1D2 * (XIO (2) +B1 (2) ) +X50 (2) +F1 (2) + (R2N2 -R1D2 *R2N6 ) *A2N (2) ) 
F2 (2) =DT* 

x (-R1D1* (XIO (2) +B1 (2) ) +X60 (2) +G1 (2) + (R2N1-R1D1*R2N6 ) *A2N (2) ) 
G2 (2) =DT* 

x (-R1D0* (XIO ( 2 ) +B1 (2) ) + (R2N0 -R1D0 *R2N6 ) *A2N(2) ) 

XXI (2) =X10 (2) +0 . 5* (B1 (2) +B2 (2) ) 

X2 (2) =X20 (2) +0 . 5* (Cl (2) +C2 (2) ) 

X3 (2) =X30 (2) +0 . 5* (D1 (2) +D2 (2) ) 

X4 (2) =X40 (2 ) +0 . 5* (El (2 ) +E2 (2) ) 

X5 (2) =X50 (2) +0 . 5* (FI (2) +F2 (2) ) 

X6 (2) =X60 (2) +0 . 5* (G1 (2) +G2 (2) ) 

C 

q* ********************** * BLOCK NO. C3 **************************** 
C FILTER FROM A2(2) TO AYSI : 

C 

B1 (3) =DT* ( -R1D5 *X10 (3) +X20 (3) + (R4N5 -R1D5 *R4N6 ) *A2NO (2) ) 

Cl (3) =DT* ( -R1D4 *X10 (3) +X30 (3) + (R4N4 -R1D4 *R4N6 ) *A2NO (2) ) 

D1 (3) =DT* ( -R1D3 *X10 (3) +X40 (3) + (R4N3 -R1D3 *R4N6 ) *A2NO (2) ) 

El (3) =DT* ( -R1D2 *X10 (3) +X50 (3) + (R4N2 -R1D2 *P4N6 ) *A2NO (2) ) 

FI (3) =DT* ( -R1D1*X10 (3) +X60 (3) + (R4N1-R1D1*P4N6 ) *A2NO (2) ) 

G1 (3) =DT* ( -R1D0 *X10 (3) + (R4N0 -R1D0 *P4N6 ) *A2NO (2) ) 

B2 (3) =DT* 

x (-R1D5* (XIO (3) +B1 (3) ) +X20 (3) +C1 (3) + (R4N5 -R1D5 *R4N6 ) *A2N(2) ) 

C2 (3) =DT* 

x ( -R1D4 * (XIO (3) +B1 (3) ) +X30 (3) +D1 (3) + (R4N4 -R1D4 *R4N6 ) *A2N(2) ) 

D2 (3) =DT* 

x ( -R1D3 * (XIO (3) +B1 (3) ) +X40 (3) +E1 (3) + (R4N3 -R1D3 *R4N6 ) *A2N(2) ) 

E2 (3) =DT* 

x ( -R1D2 * (XIO (3) +B1 (3) ) +X50 (3) +F1 (3) + (R4N2 -R1D2 *R4N6 ) *A2N(2) ) 

F2 (3) =DT* 

x (-R1D1* (XIO (3) +B1 (3) ) +X60 (3) +G1 (3) + (R4N1-R1D1*R4N6 ) *A2N(2) ) 

G2 (3) =DT* 

x (-R1D0* (XIO (3) +B1 (3) ) + (R4N0 -R1D0 *R4N6 ) *A2N (2) ) 

XXI (3) =X10 (3) +0 . 5* (B1 (3) +B2 (3) ) 

X2 (3) =X20 (3) +0 . 5* (Cl (3) +C2 (3) ) 

X3 (3) =X30 (3) +0 . 5* (D1 (3) +D2 (3) ) 

X4 (3) =X40 (3) +0 . 5* (El (3) +E2 (3) ) 
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X5 (3) =X50 (3) +0 . 5* (FI (3) +F2 (3) ) 

X6 (3) =X60 (3) +0 . 5* (G1 (3) +G2 (3) ) 

C 

q * *********************** BLOCK NO. C4 **************************** 

C FILTER FROM THETAAD TO THETASD : 

C 

B1 (4) =DT* ( - P1D5 *X10 (4) +X20 (4) + (P1N5 -P1D5*P1N6 ) *BADNO (2) ) 

Cl (4) =DT* ( - P1D4 *X10 (4) +X30 (4) + (P1N4 -P1D4*P1N6 ) *BADNO (2) ) 

D1 (4) =DT* ( - P1D3 *X10 (4) +X40 (4) + ( P1N3 - P1D3 *P1N6 ) *BADNO (2) ) 

El (4) =DT* ( - P1D2 *X10 (4) +X50 (4) + (P1N2 -P1D2*P1N6 ) *BADNO (2) ) 

FI (4) =DT* (-P1D1*X10 (4) +X60 (4) + (P1N1 -P1D1*P1N6 ) *BADNO (2) ) 

G1 (4) =DT* ( - P1D0 *X10 (4) + (PINO -P1D0*P1N6 ) *BADNO (2) ) 

B2 (4) =DT* 

x (-P1D5* (XIO (4) +B1 (4) ) +X20 (4) +C1 (4) + ( P1N5 - P1D5*P1N6 ) * B ADN ( 2 ) ) 
C2 (4) =DT* 

x (-P1D4* (XIO (4) +B1 (4) ) +X30 (4) +D1 (4) + ( P1N4 - P1D4 *P1N6 ) * B ADN ( 2 ) ) 
D2 (4) =DT* 

x (-P1D3* (XIO (4) +B1 (4) ) +X40 (4) +E1 (4) + ( P1N3 - P1D3 *P1N6 ) * B ADN ( 2 ) ) 
E2 (4) =DT* 

x (-P1D2* (XIO (4) +B1 (4) ) +X50 (4) +F1 (4) + ( P1N2 - P1D2 *P1N6 ) * B ADN ( 2 ) ) 
F2 (4) =DT* 

x (-P1D1* (XIO (4) +B1 (4) ) +X60 (4) +G1 (4) + ( P1N1 - P1D1*P1N6 ) *BADN (2 ) ) 
G2 (4) =DT* 

x (-P1D0* (XIO (4) +B1 (4) ) + ( PINO - P1D0 *P1N6 ) * B ADN ( 2 ) ) 


XXI (4) =X10 (4) +0 . 5* (B1 (4) +B2 (4) ) 

X2 (4) =X20 (4) +0 . 5* (Cl (4) +C2 (4) ) 

X3 (4) =X30 (4) +0 . 5* (D1 (4) +D2 (4) ) 

X4 (4) =X40 (4 ) +0 . 5* (El (4 ) +E2 (4) ) 

X5 (4) =X50 (4) +0 . 5* (FI (4) +F2 (4) ) 

X6 (4) =X60 (4) +0 . 5* (G1 (4) +G2 (4) ) 

C 

C FILTER FROM A2(l) TO THETASD: 

C 

B1 (5) =DT* ( - P1D5 *X10 (5) +X20 (5) + (P2N5 -P1D5*P2N6 ) *A2NO (1) ) 
Cl (5) =DT* ( - P1D4 *X10 (5) +X30 (5) + (P2N4 -P1D4*P2N6 ) *A2NO (1) ) 
D1 (5) =DT* ( - P1D3 *X10 (5) +X40 (5) + ( P2N3 - P1D3 *P2N6 ) *A2NO (1) ) 
El (5) =DT* ( - P1D2 *X10 (5) +X50 (5) + (P2N2 -P1D2*P2N6 ) *A2NO (1) ) 
FI (5) =DT* (-P1D1*X10 (5) +X60 (5) + ( P2N1 - P1D1*P2N6 ) *A2NO (1) ) 
G1 (5) =DT* ( - P1D0 *X10 ( 5 ) + (P2N0 -P1D0*P2N6 ) *A2NO (1) ) 


B2 (5) =DT* 

x (-P1D5* (XIO (5) +B1 (5) ) +X20 (5) +C1 (5) + ( P2N5 - P1D5*P2N6 ) *A2N (1) ) 
C2 (5) =DT* 

x (-P1D4* (XIO (5) +B1 (5) ) +X30 (5) +D1 (5) + ( P2N4 - P1D4 *P2N6 ) *A2N (1) ) 
D2 (5) =DT* 

x (-P1D3* (XIO (5) +B1 (5) ) +X40 (5) +E1 (5) + ( P2N3 - P1D3 *P2N6 ) *A2N (1) ) 
E2 (5) =DT* 

x (-P1D2* (XIO (5) +B1 (5) ) +X50 (5) +F1 (5) + ( P2N2 - P1D2 *P2N6 ) *A2N (1) ) 
F2 (5) =DT* 

x (-P1D1* (XIO (5) +B1 (5) ) +X60 (5) +G1 (5) + ( P2N1 - P1D1*P2N6 ) *A2N (1) ) 
G2 (5) =DT* 

x (-P1D0* (XIO (5) +B1 (5) ) + (P2N0-P1D0*P2N6) *A2N (1) ) 


XXI (5) =X10 (5) +0 . 5* (B1 (5) +B2 (5) ) 
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X2 (5) =X20 (5) +0 . 5* (Cl (5) +C2 (5) ) 

X3 (5) =X30 (5) +0 . 5* (D1 (5) +D2 (5) ) 

X4 (5) =X40 (5) +0 . 5* (El (5) +E2 (5) ) 

X5 (5) =X50 (5) +0 . 5* (FI (5) +F2 (5) ) 

X6 (5) =X60 (5) +0 . 5* (G1 (5) +G2 (5) ) 

C 

C FILTER FROM A2(l) TO AXSI : 

C 

B1 (6) =DT* (-P1D5*X10 (6) +X20 (6) + (P4N5 -P1D5*P4N6 ) *A2NO (1) ) 

Cl (6) =DT* (-P1D4*X10 (6) +X30 (6) + (P4N4 -P1D4*P4N6 ) *A2NO (1) ) 

D1 (6) =DT* ( - P1D3 *X10 ( 6 ) +X40 (6) + ( P4N3 - P1D3 *P4N6 ) *A2NO (1) ) 

El (6) =DT* ( - P1D2 *X10 ( 6 ) +X50 (6) + (P4N2 -P1D2*P4N6 ) *A2NO (1) ) 

FI (6) =DT* (-P1D1*X10 (6) +X60 (6) + ( P4N1 - P1D1*P4N6 ) *A2NO (1) ) 

G1 (6) =DT* ( - P1D0 *X10 ( 6 ) + (P4N0 -P1D0*P4N6 ) *A2NO (1) ) 

B2 (6) =DT* 

x (-P1D5* (XIO (6) +B1 (6) ) +X20 (6) +C1 (6) + ( P4N5 - P1D5 *P4N6 ) *A2N (1) ) 

C2 (6) =DT* 

x (-P1D4* (XIO (6) +B1 (6) ) +X30 (6) +D1 (6) + ( P4N4 - P1D4 *P4N6 ) *A2N (1) ) 

D2 (6) =DT* 

x (-P1D3* (XIO (6) +B1 (6) ) +X40 (6) +E1 (6) + ( P4N3 - P1D3 *P4N6 ) *A2N (1) ) 

E2 (6) =DT* 

x (-P1D2* (XIO (6) +B1 (6) ) +X50 (6) +F1 (6) + ( P4N2 - P1D2 *P4N6 ) *A2N (1) ) 

F2 (6) =DT* 

x (-P1D1* (XIO (6) +B1 (6) ) +X60 (6) +G1 (6) + ( P4N1 - P1D1*P4N6 ) *A2N (1) ) 

G2 (6) =DT* 

x (-P1D0* (XIO (6) +B1 (6) ) + (P4N0-P1D0*P4N6) *A2N (1) ) 

XXI (6) =X10 (6) +0 . 5* (B1 (6) +B2 (6) ) 

X2 (6) =X20 (6) +0 . 5* (Cl (6) +C2 (6) ) 

X3 (6) =X30 (6) +0 . 5* (D1 (6) +D2 (6) ) 

X4 (6) =X40 (6) +0 . 5* (El (6) +E2 (6) ) 

X5 (6) =X50 (6) +0 . 5* (FI (6) +F2 (6) ) 

X6 (6) =X60 (6) +0 . 5* (G1 (6) +G2 (6) ) 

C 

C FILTER FROM PS I AD 2 TO PSISD2 : 

C 

B1 (7) =DT* (-Y1D3*X10 (7) +X20 (7) + ( Y1N3 - Y1D3 *Y1N4 ) *BADNO (3) ) 

Cl (7) =DT* (-Y1D2*X10 (7) +X30 (7) + ( Y1N2 - Y1D2 *Y1N4 ) *BADNO (3) ) 

D1 (7) =DT* (-Y1D1*X10 (7) +X40 (7) + (Y1N1-Y1D1*Y1N4) *BADNO (3) ) 

El (7) =DT* (-YlD0*XlO (7) + ( Y1N0 - Y1D0 *Y1N4 ) *BADNO (3) ) 

B2 (7) =DT* 

x ( - Y1D3 * (XIO (7) +B1 (7) ) +X20 (7) +C1 (7) + ( Y1N3 - Y1D3 *Y1N4 ) *BADN (3) ) 

C2 (7) =DT* 

x ( - Y1D2 * (XIO (7) +B1 (7) ) +X30 (7) +D1 (7) + ( Y1N2 - Y1D2 *Y1N4 ) *BADN (3) ) 

D2 (7) =DT* 

x (-Y1D1* (XIO (7) +B1 (7) ) +X40 (7) +E1 (7) + (Y1N1-Y1D1*Y1N4) * B ADN ( 3 ) ) 

E2 (7) =DT* 

x ( - Y1D0 * (XIO (7) +B1 (7) ) + ( Y1N0 - Y1D0 *Y1N4 ) * B ADN ( 3 ) ) 

XXI (7) =X10 (7) +0 . 5* (B1 (7) +B2 (7) ) 

X2 (7) =X20 (7) +0 . 5* (Cl (7) +C2 (7) ) 

X3 (7) =X30 (7) +0 . 5* (D1 (7) +D2 (7) ) 

X4 (7) =X40 (7) +0 . 5* (El (7) +E2 (7) ) 
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c 

q * *********************** BLOCK NO. C8 **************************** 
C FILTER FROM A2(3) TO AZSI : 

C 


B1 ( 

8 

) =DT* ( -H1D3 *X10 ( 8 

) +X20 (8) 

1 + 

(H1N3 

-H1D3 *H1N4 ) *A2NO (3) 

) 


Cl ( 

8 

) =DT* ( -H1D2 *X10 ( 8 

) +X30 (8) 

1 + 

(H1N2 

-H1D2 *H1N4 ) *A2NO (3) 

) 


D1 ( 

8 

) =DT* ( -H1D1*X10 ( 8 

) +X40 (8) 

1 + 

(H1N1 

-H1D1*H1N4 ) *A2NO (3) 

) 


El ( 

8 

) =DT* ( -H1D0 *X10 ( 8 

) + 


(H1N0 

-H1D0 *H1N4 ) *A2NO (3) 

) 


B2 ( 

8 

) =DT* 







X 

( 

-H1D3 * (XIO (8) +B1 ( 

8) ) +X20 

(8 

) +C1 ( 

8) + (H1N3 -H1D3 *H1N4 ) 

*A2N 

(3) ) 

C2 ( 

8 

) =DT* 







X 

( 

-H1D2 * (XIO (8) +B1 ( 

8) ) +X30 

(8 

) +D1 ( 

8) + (H1N2 -H1D2 *H1N4 ) 

*A2N 

(3) ) 

D2 ( 

8 

) =DT* 







X 

( 

-H1D1* (XIO (8) +B1 ( 

8) ) +X40 

(8 

) +E1 ( 

8) + (H1N1-H1D1*H1N4 ) 

*A2N 

(3) ) 

E2 ( 

8 

) =DT* 







X 

( 

-H1D0 * (XIO (8) +B1 ( 

8) ) + (H1N0 

-H1D0 

*H1N4 ) *A2N ( 3 ) ) 



XXI ( 

8) =X10 (8) +0 . 5* (B1 

(8) +B2 (8) ) 




X2 ( 

8 

) =X20 (8) +0 . 5* (Cl ( 

8) +C2 (8) ) 





X3 ( 

8 

) =X30 (8) +0 . 5* (D1 ( 

8) +D2 (8) ) 





X4 ( 

8 

) =X40 (8) +0 . 5* (El ( 

8) +E2 (8) ) 






C 

C COMPUTE STATE VARIABLES TO OBTAIN: 

C SIMULATOR TRANS ACCEL 

C SIMULATOR ANGULAR VEL (TILT AND PURE) 

C 

BSDT (1) =XX1 (2) +R2N6 *A2N ( 2 ) 

BSDT (2) =XX1 (5) +P2N6 *A2N ( 1 ) 

BSDR ( 1 ) =XX1 ( 1 ) +R1N6 *BADN ( 1 ) 

BSDR ( 2 ) =XX1 (4) +P1N6*BADN (2) 

BSDR ( 3 ) =XX1 ( 7 ) +Y1N4 *BADN ( 3 ) 

ASI (1) =XX1 (6) + P4N6 *A2N ( 1 ) 

AS I (2) =XX1 (3) + R4N6 *A2N ( 2 ) 

ASI (3) =XX1 (8) +H1N4 *A2N ( 3 ) 

C 

C UPDATE ALL THE DUMMY VARIABLES : 

C 

DO 1=1,8 

XIO (I) =XX1 (I) 

X20 (I) =X2 (I) 

X30 (I) =X3 (I) 

X40 (I) =X4 (I) 

END DO 
C 

DO 1=1,6 

X50 (I) =X5 (I) 

X60 (I) =X6 (I) 

END DO 
C 

DO I J=1 , 3 

A2NO ( I J) = A2N ( I J) 

BADNO(IJ) = BADN(IJ) 

END DO 
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RETURN 

END 
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8.14. resetc2.f 


SUBROUTINE RESETC2 
C 

C THIS ROUTINE: 

C 

C (1) DOES T=0 INITIALIZATION OF MOTION VARIABLES 
C 

C (2) USES A SECOND ORDER SCHEME TO DRIVE TO THE NEUTRAL 
C POSITION. (OVERDAMPED OSCILLATOR; ZETA = 1.5, OMEGAN = 1.0 ) 

C 

INCLUDE 1 comint 2 . com ' 

INCLUDE 'wcom2.com' 

INCLUDE 'matrixlc.com' 


Qic i( ic ic ic ic ic ic i( ic ic ic i( ic ic ic ic ic ic ic ic ic ic ic i( ic ic it ic i( ic ic ic ic ic ic ic ic ic ic i( ic ic ic ic ic ic ic ic ic ic ic ic ic ic ic ie ic ic it ic i( ic ic ic ic ic ic i( 

C 

C GAINS, ACCELERATION AND VELOCITY LIMITS FOR SECOND ORDER. 

C 

C VALUES FOR ACCELERATION AND VELOCITY ARE METERS/SEC**2 

C AND METERS/SEC. ROTATIONS AND ROTATIONAL VELOCITIES 

C ARE IN RADIANS AND RADIANS / SECOND . 

C 

Qic it ic it ic ic ic it ic it ic it ic ic ic ic ic it ic ic i( ic ic it ic it ic ic ic ic ic ic ic it ic it ic ic ic it ic it ic it ic ic ic ic ic ic ic ic i( ic ic it ic ic ic ic i( ic ic ic ic it ic ic i( ic 

c 

C .03492 RADIANS/ SEC**2 ==2.0 DEG/SEC**2 

C .294 METERS/ SEC**2 == . 03 G 

C 

PARAMETER (A_ACCLIM = .03491) 

PARAMETER (T_ACCLIM = .294 ) 

C 

C THESE PARAMETERS 

C 

C .2617 RAD/SEC == 

C .610 METERS/SEC 

C 

PARAMETER (AJVELLIM = .2617 ) 

PARAMETER (TJVELLIM = .610 ) 

C 

C VALUES FOR X FILTER 

C 

DATA XDDLIM / T_ACCLIM / 

DATA XDLIM / TJVELLIM / 

C 

C VALUES FOR Y FILTER 

C 

DATA YDDLIM / T_ACCLIM / 

DATA YDLIM / TJVELLIM / 

C 

C VALUES FOR Z FILTER 

C 


DATA 

ZDDLIM 

/ 

T_ 

_ACCLIM 

/ 

DATA 

ZDLIM 

/ 

T_ 

_VELLIM 

/ 


C 


ARE SET TO THE PERFORMANCE LIMIT OF THE BASE 
15 DEG/SEC 
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C VALUES FOR PSI FILTER 

C 

DATA PSIDDLIM / A_ACCLIM / 

DATA PSIDLIM / A_VELLIM / 

C 

C VALUES FOR THETA FILTER 

C 

DATA THEDDLIM / A_ACCLIM / 

DATA THEDLIM / A_VELLIM / 

C 

C VALUES FOR PHI FILTER 

C 

DATA PH I DDL I M / A_ACCLIM / 

DATA PHIDLIM / A_VELLIM / 

DATA TWOZOMGN/ 3.0 / 

C 

C local functions 

CL I MIT (X , XMIN , XMAX) = MIN ( MAX ( X, XMIN ), XMAX ) 

C 

C If last operate run ended braked, reset braking algorithm 

C to unbraked state 

C 

IF (FLAG2.EQ.1) THEN 
X = SSI (1) 

Y = SSI (2) 

Z = SSI (3) 

PHI = BETAS (1) 

THE = BETAS (2) 

PSI = BETAS (3) 

DO I J=1 , 6 

RLID (IJ) =0 . 

RLIDO ( I J) =0 . 

END DO 
IT2 = 0 
FLAG2=0 
END IF 


C 

C 

C 


T = 0 INITIALIZATION; 


T 

= 

0 . 

INT 

= 

0 

DXDLX 

= 

0 . 

DXDLXD 

= 

0 . 

DXDDX 

= 

0 . 

DXDDXD 

= 

0 . 

DTHDDX 

= 

0 . 

LAMX 

= 

LAMXO 

DELX 

= 

DELXO 

DYDLY 

= 

0 . 

DYDLYD 

= 

0 . 

DYDDY 

= 

0 . 

DYDDYD 

= 

0 . 

DPHDDY 

= 

0 . 

LAMY 

= 

LAMYO 

DELY 

= 

DELYO 

DZDEZ 

= 

0 . 
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DZDEZD = 0 . 

ETAZ = ETAZO 

DPSDE = 0. 

ETAPS = ETAPSO 

C 

C FADE TO THE NEUTRAL POSITION USING SECOND ORDER FILTER TO ROVIDE 

C VELOCITY AND ACCELERATION CONTROL. 

C 

C 

C DRIVE X TO NEUTRAL POSITION (X = 0) 

C 

XDDF = -X - TWOZOMGN*XD 

XDD = CLIMIT (XDDF, -XDDLIM , XDDLIM) 

XDF = XD + XDD*H 

XD = CLIMIT (XDF, -XDLIM , XDLIM) 

X = X + XD*H 

C 

C DRIVE Y TO NEUTRAL POSITION (Y = 0) 

C 

YDDF = -Y - TWOZOMGN* YD 

YDD = CLIMIT (YDDF, - YDDLIM , YDDLIM) 

YDF = YD + YDD*H 

YD = CLIMIT (YDF, - YDLIM , YDLIM) 

Y = Y + YD*H 

C 

C DRIVE Z TO NEUTRAL POSITION (Z = 0) 

C 

ZDDF = -Z - TWOZOMGN* ZD 

ZDD = CLIMIT (ZDDF, -ZDDLIM, ZDDLIM) 

ZDF = ZD + ZDD*H 

ZD = CLIMIT (ZDF, -ZDLIM, ZDLIM) 

Z = Z + ZD*H 

C 

C DRIVE PSI TO NEUTRAL POSITION (PSI = 0) 

C 

PSIDDF = -PSI - TWOZOMGN* PS ID 

PSIDDFL = CLIMIT (PSIDDF, -PSIDDLIM, PSIDDLIM) 

PSIDF = PSID + PSIDDFL*H 

PSID = CLIMIT (PSIDF, -PSIDLIM, PSIDLIM) 

PSI = PSI + PSID*H 

C 

C DRIVE THETA TO NEUTRAL POSITION (THETA = 0) 

C 

THEDDF = -THE - TWOZOMGN* THED 

THEDDFL = CLIMIT (THEDDF , -THEDDLIM , THEDDLIM) 

THEDF = THED + THEDDFL*H 

THED = CLIMIT (THEDF, -THEDLIM, THEDLIM) 

THE = THE + THED*H 

C 

C DRIVE PHI TO NEUTRAL POSITION (PHI = 0) 

C 

PHIDDF = -PHI - TWOZOMGN* PHID 

PHIDDFL = CLIMIT (PHIDDF, -PHIDDLIM, PHIDDLIM) 

PHIDF = PHID + PHIDDFL*H 

PHID = CLIMIT (PHIDF, -PHIDLIM, PHIDLIM) 

PHI = PHI + PHID*H 
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c 

C DUMMY INTEGRATIONS 

C 

XD1 = XD 

DXDLXD1 = DXDLXD 
DXDDXD1 = DXDDXD 
YD1 = YD 

DYDLYD1 = DYDLYD 
DYDDYD1 = DYDDYD 
ZD1 = ZD 

DZDEZD1 = DZDEZD 
RETURN 
END 
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8.15. simq.f 

SUBROUTINE SIMQ (A, B , N , KS ) 

DIMENSION A (36 ) , B ( 6 ) 

TOL=0 . 

KS = 0 
JJ= -N 

DO 65 J=1,N 
JY= J+l 
JJ= JJ+N+1 
BIGA=0 . 

IT=JJ-J 
DO 30 I=J,N 
IJ=IT+I 

IF (ABS (BIGA) -ABS (A(IJ) ) ) 20,30,30 
20 BIGA=A ( I J) 

IMAX=I 

30 CONTINUE 

IF (ABS (BIGA) -TOL) 35,35,40 
35 KS=1 

RETURN 

40 I 1= J+N* ( J- 2 ) 

IT=IMAX- J 
DO 50 K= J, N 
I1=I1+N 
I2=I1+IT 
SAVE 1 = A (II) 

A ( I 1 ) =A ( 12 ) 

A (12) = SAVE 1 

50 A(I1) =A(I1) /BIGA 

SAVE1=B (IMAX) 

B (IMAX) =B (J) 

B (J) = SAVE 1 /BIGA 
IF(J-N) 55,70,55 
55 IQS=N*(J-1) 

DO 65 IX= JY, N 
IXJ=IQS+IX 
IT= J- IX 
DO 60 JX= JY, N 

IXJX=N* ( JX-1) +IX 
JJX=IXJX+IT 

60 A(IXJX) =A(IXJX) - (A(IXJ) *A(JJX) ) 

65 B (IX) =B (IX) - (B (J) *A(IXJ) ) 

70 NY=N- 1 
IT=N*N 

DO 80 J= 1 , NY 
IA=IT- J 
IB=N- J 
IC=N 

DO 80 K=1 , J 

B (IB) =B (IB) -A(IA) *B (IC) 

IA=IA-N 

80 IC=IC-1 
RETURN 
END 
C 
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8.16. state4.f 


c 

C NONLINEAR STATE SPACE FILTERS (ALL IN INERTIAL FRAME) : 

C 

SUBROUTINE STATE4 
INCLUDE ' optint3 . com' 

INCLUDE 'nopt4.com' 

REAL ABK1X (6, 6) , BK2X (6, 3) ,BK3X (6) ,XXI1 (9) ,XXI2 (9) 

REAL ABK1Y (6, 6) , BK2Y (6, 3) , BK3Y (6) , XYI1 (9) ,XYI2 (9) 

REAL XZI1 (5) , XZI2 (5) ,XRI1 (4) ,XRI2 (4) 

C 

C ROLL AND PITCH UNITY GAIN FILTERS 

C 

BSDR (1) =BADNO (1) 

BSDR (2) =BADNO (2) 

C 

C 9TH ORDER SURGE/PITCH STATE SPACE FILTER 

C 

DO 1=1,6 
DO J=l, 6 

K1X (1, J) =R2IX (1) * 

x (BVX (1, 1) *PX (1, J) +BVX (2, 1) *PX (2, J) +BVX (3 , 1 ) *PX (3 , J) 

x +BVX (4, 1) *PX (4, J) +BVX (5 , 1 ) *PX (5 , J) +BVX (6, 1) *PX (6, J) 

x +DQCX ( J) ) 

K1X (2, J) =R2 IX (2) * 

x (BVX (1, 2) *PX (1, J) +BVX (2, 2) *PX (2, J) +BVX (3 , 2) *PX (3 , J) 

x +BVX (4, 2) *PX (4, J) +BVX (5 , 2) *PX (5 , J) +BVX (6, 2) *PX (6, J) 

x +PX ( 9 , J) ) 

ABK1X ( I , J) = 

x AVX (I , J) -BVX (I , 1) *K1X (1, J) -BVX (I , 2) *K1X (2, J) 

END DO 
END DO 

DO 1=1,6 
DO J=1 , 3 

K2X (1, J) =R2IX (1) * 

x (BVX (1, 1) *PX (1, J+6) +BVX (2 , 1) *PX (2, J+6) +BVX (3 , 1 ) *PX (3 , J+6 ) 

x +BVX (4, 1) *PX (4, J+6) +BVX (5 , 1 ) *PX (5 , J+6 ) +BVX (6 , 1) *PX (6 , J+6 ) ) 

K2X (2, J) =R2 IX (2) * 

x (BVX (1, 2) *PX (1, J+6) +BVX (2 , 2 ) *PX (2 , J+6 ) +BVX (3 , 2 ) *PX (3 , J+6 ) 

x +BVX (4,2) *PX (4, J+6) +BVX (5, 2) *PX (5, J+6) +BVX (6, 2) *PX (6, J+6) 

x +PX ( 9 , J+6 ) ) 

BK2X (I , J) =BVX (I , 1) *K2X (1, J) +BVX (1,2) *K2X (2, J) 

END DO 
END DO 

K3X (1) =R2IX (1) * 

x (BVX (1, 1) *PX (1, 11) +BVX (2,1) *PX ( 2 , 11 ) +BVX (3,1)*PX(3,11) 


101 



x +BVX (4,1) *PX (4 , 11 ) +BVX (5,1)*PX(5,11) +BVX (6 , 1) *PX (6, 11) ) 


K3X (2) =R2 IX (2) * 

x (BVX (1, 2) *PX (1, 11) +BVX (2,2) *PX (2, 11 ) +BVX (3,2)*PX(3,11) 
x +BVX (4,2) *PX (4 , 11 ) +BVX (5, 2) *PX (5, 11) +BVX (6, 2) *PX (6, 11) 
x +PX (9,11) ) 

DO 1=1,6 

BK3X (I) =BVX (I , 1) *K3X (1) +BVX (I, 2) * (1 . 0+K3X (2) ) 

END DO 

DO 1=1,6 

XXII (I) =DT* 

x (ABK1X (1,1) *XXO ( 1 ) +ABK1X (1,2) *XXO ( 2 ) +ABK1X (1,3) *XXO ( 3 ) 
x +ABK1X (1,4) *XXO (4) +ABK1X (1,5) *XXO (5) +ABK1X (I, 6) *XXO (6) 
x -BK2X (I , 1) *XXO (7) -BK2X (1,2) *XXO (8) -BK2X (I, 3) *XXO (9) 
x -BK3X (I) *A2NO (1) ) 

END DO 

XXII (7) =DT*XXO (8) 

XXII (8) =DT*XXO (9) 

XXII (9) =DT* 

x ( -K1X (2,1) *XXO (1) -K1X (2,2) *XXO (2) -K1X (2,3) *XXO (3) 
x -K1X (2,4) *XXO (4) -K1X (2,5) *XXO (5) -K1X (2,6) *XXO (6) 
x -K2X (2,1) *XXO (7) -K2X (2,2) *XXO (8) -K2X (2,3) *XXO (9) 
x -K3X (2) *A2NO (1) ) 

DO 1=1,6 

XXI 2 (I) =DT* 

x (ABK1X (I , 1) * (XXO (1) +XXI1 (1) ) +ABK1X (1,2) * (XXO (2) +XXI1 (2) ) 
x +ABK1X (1,3) * (XXO (3) +XXI1 (3) ) +ABK1X (1,4) * (XXO (4) +XXI1 (4) ) 
x +ABK1X (I , 5) * (XXO (5) +XXI1 (5) ) +ABK1X (I , 6) * (XXO (6) +XXI1 (6) ) 
x -BK2X (I, 1) * (XXO (7) +XXI1 (7) ) -BK2X (I , 2) * (XXO (8) +XXI1 (8) ) 
x -BK2X (1,3) * (XXO (9) +XXI1 (9) ) -BK3X (I) *A2N (1) ) 

END DO 

XXI 2 (7) =DT* (XXO (8) +XXI1 (8) ) 

XXI 2 (8) =DT* (XXO (9) +XXI1 (9) ) 

XXI 2 (9) =DT* ( -K1X (2,1)* (XXO (1) +XXI1 (1) ) 
x -K1X (2,2)* (XXO (2) +XXI1 (2) ) -K1X (2,3)* (XXO (3) +XXI1 (3) ) 
x -K1X (2,4)* (XXO (4) +XXI1 (4) ) -K1X (2,5)* (XXO (5) +XXI1 (5) ) 
x -K1X (2,6)* (XXO (6) +XXI1 (6) ) -K2X (2,1)* (XXO (7) +XXI1 (7) ) 
x -K2X (2,2)* (XXO (8) +XXI1 (8) ) -K2X (2,3)* (XXO (9) +XXI1 (9) ) 
x -K3X (2) *A2N (1) ) 

DO 1=1,9 

XX (I) =XXO (I) +0.5* (XXII (I) +XXI2 (I) ) 

END DO 
C 

C 9TH ORDER SWAY/ROLL STATE SPACE FILTER 

C 

DO 1=1,6 
DO J=l, 6 

K1Y ( 1 , J) =R2 IY ( 1 ) * 

x ( B VY (1,1) *PY ( 1 , J) +BVY (2,1) *PY (2 , J) +BVY(3, 1) *PY (3 , J) 

x +BVY (4 , 1) *PY(4, J) +BVY ( 5 , 1) *PY (5, J) +BVY(6, 1) *PY(6, J) 

x +DQCY ( J) ) 

K1Y (2, J) =R2IY(2) * 
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x (BVY (1, 2) *PY (1, J) +BVY (2, 2) *PY (2, J) +BVY (3,2) *PY (3 , J) 

x +BVY (4,2) *PY (4, J) +BVY (5, 2) *PY (5, J) +BVY (6, 2) *PY (6, J) 

x +PY ( 9 , J) ) 

ABK1Y ( I , J) = 

x AVY (I , J) -BVY (1 , 1) *K1Y (1, J) -BVY (1 , 2) *K1Y (2, J) 

END DO 
END DO 

DO 1=1,6 

DO J=1 , 3 

K2Y ( 1 , J) =R2 IY ( 1 ) * 

x (BVY (1, 1) *PY (1, J+6) +BVY (2, 1) *PY(2, J+6) +BVY (3 , 1 ) *PY (3 , J+6 ) 

x +BVY (4,1) *PY(4, J+6) +BVY(5, 1) *PY (5 , J+6 ) +BVY (6,1) *PY(6, J+6) ) 

K2Y (2 , J) =R2 IY ( 2 ) * 

x (BVY (1, 2) *PY (1, J+6) +BVY (2, 2) *PY (2, J+6) +BVY (3,2) *PY(3 , J+6) 

x +BVY (4, 2) *PY (4, J+6) +BVY (5, 2) *PY (5, J+6) +BVY (6, 2) *PY (6, J+6) 

x +PY ( 9 , J+6 ) ) 

BK2Y (I, J) =BVY (I , 1) *K2Y (1, J) +BVY (I, 2) *K2Y (2, J) 

END DO 
END DO 

K3Y (1) =R2 IY ( 1 ) * 

x ( BVY ( 1 , 1) *PY(1, 11) +BVY ( 2 , 1) *PY(2, 11) +BVY(3, 1) *PY(3, 11) 

x +BVY (4 , 1) *PY(4, 11) +BVY ( 5 , 1) *PY(5, 11) +BVY(6, 1) *PY(6, 11) ) 

K3Y (2) =R2IY (2) * 

x (BVY (1, 2) *PY (1, 11) +BVY (2,2) *PY (2, 11) +BVY(3 , 2) *PY (3 , 11) 

x +BVY (4,2) *PY (4 , 11 ) +BVY (5, 2) *PY (5, 11) +BVY (6, 2) *PY (6, 11) 

x +PY (9,11) ) 

DO 1=1,6 

BK3Y (I) =BVY (I , 1) *K3Y (1) +BVY (I, 2) * (1 . 0+K3Y (2) ) 

END DO 

DO 1=1,6 

XYI 1 (I) =DT* 

x (ABK1Y (1,1) *XYO ( 1 ) +ABK1Y (1,2) *XYO ( 2 ) +ABK1Y (1,3) *XYO ( 3 ) 
x +ABK1Y (1,4) *XYO (4) +ABK1Y (1,5) *XYO (5) +ABK1Y (I , 6) *XYO (6) 
x -BK2Y (1,1) *XYO ( 7 ) -BK2Y (1,2) *XYO ( 8 ) -BK2Y (1,3) *XYO ( 9 ) 
x -BK3Y (I) *A2NO (2) ) 

END DO 

XYI 1 (7) =DT*XYO (8) 

XYI 1 (8) =DT*XYO (9) 

XYI 1 (9) =DT* 

x ( -K1Y (2 , 1) *XYO (1) -K1Y (2, 2) *XYO (2) -K1Y(2, 3) *XYO (3) 
x -K1Y (2,4) *XYO (4) -K1Y (2, 5) *XYO (5) -K1Y(2, 6) *XYO (6) 

x -K2Y (2 , 1) *XYO (7) -K2Y (2, 2) *XYO (8) -K2Y(2, 3) *XYO (9) 

x -K3Y (2) *A2NO (2) ) 

DO 1=1,6 

XYI 2 (I) =DT* 

x (ABK1Y (I , 1) * (XYO (1) +XYI1 (1) ) +ABK1Y (I, 2) * (XYO (2) +XYI1 (2) ) 

x +ABK1Y (1,3) * (XYO (3) +XYI1 (3) ) +ABK1Y (I , 4) * (XYO (4) +XYI1 (4) ) 

x +ABK1Y (I, 5) * (XYO (5) +XYI1 (5) ) +ABK1Y (I, 6) * (XYO (6) +XYI1 (6) ) 
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x -BK2Y (I , 1) * (XYO (7) +XYI1 (7) ) -BK2Y (1,2) * (XYO (8) +XYI1 (8) ) 

x -BK2Y (1,3) * (XYO (9) +XYI1 (9) ) -BK3Y (I) *A2N (2) ) 

END DO 

XYI2 (7) =DT* (XYO (8) +XYI1 (8) ) 

XYI2 (8) =DT* (XYO (9) +XYI1 (9) ) 

XYI2 (9) =DT* ( -K1Y (2 , 1) * (XYO (1) +XYI1 (1) ) 
x -K1Y (2, 2) * (XYO (2) +XYI1 (2) ) -K1Y (2, 3) * (XYO (3) +XYI1 (3) ) 
x -K1Y (2, 4) * (XYO (4) +XYI1 (4) ) -K1Y (2, 5) * (XYO (5) +XYI1 (5) ) 
x -K1Y (2, 6) * (XYO (6) +XYI1 (6) ) -K2Y (2 , 1) * (XYO (7) +XYI1 (7) ) 
x -K2Y (2, 2) * (XYO (8) +XYI1 (8) ) -K2Y (2, 3) * (XYO (9) +XYI1 (9) ) 
x -K3Y (2) *A2N (2) ) 

DO 1=1,9 

XY (I) =XYO (I) +0.5* (XYI 1 (I) +XYI2 (I) ) 

END DO 
C 

C 5TH ORDER HEAVE STATE SPACE FILTER 

C 

K1Z (1) =BVZ (1) *PZ (1, 1) +BVZ (2) *PZ (2, 1) +PZ (5,1) 

K1Z (2) =BVZ (1) *PZ (1, 2) +BVZ (2) *PZ (2, 2) +PZ (5,2) 

K2Z (1) =BVZ (1) *PZ (1, 3) +BVZ (2) *PZ (2, 3) +PZ (5,3) 

K2Z (2) =BVZ (1) *PZ (1, 4) +BVZ (2) *PZ (2, 4) +PZ (5,4) 

K2Z (3) =BVZ (1) *PZ (1, 5) +BVZ (2) *PZ (2, 5) +PZ (5,5) 

K3Z=BVZ (1) *PZ (1, 6) +BVZ (2) *PZ (2, 6) +PZ (5,6) 

DO 1=1,2 
XZI1 (I) =DT* 

x ( (AVZ (1,1) -BVZ (I) *K1Z (1) ) *XZO (1) 
x + (AVZ (1,2) -BVZ (I) *K1Z (2) ) *XZO (2) 

x -BVZ (I) * (K2Z (1) *XZO (3) +K2Z (2) *XZO (4) +K2Z (3) *XZO (5) ) 
x -BVZ (I) * (1+K3Z) *A2NO (3) ) 

END DO 

XZI1 (3) =DT*XZO (4) 

XZI1 (4) =DT*XZO (5) 

XZI1 (5) =DT* (-K1Z (1) *XZO (1) -K1Z (2) *XZO (2) -K2Z (1) *XZO (3) 
x -K2Z (2) *XZO (4) -K2Z (3) *XZO (5) -K3Z*A2NO (3) ) 


DO 1=1,2 
XZI2 (I) =DT* 

x ( (AVZ (1,1) -BVZ (I) *K1Z (1) ) * (XZO (1) +XZI1 (1) ) 
x + (AVZ (1,2) -BVZ (I) *K1Z (2) ) * (XZO (2) +XZI1 (2) ) 
x -BVZ (I) * (K2Z (1) * (XZO (3) +XZI1 (3) ) +K2Z (2) * (XZO (4) +XZI1 (4) ) 
x +K2Z (3) * (XZO (5) +XZI1 (5) ) ) 

x -BVZ (I) * (1+K3Z) *A2N(3) ) 

END DO 

XZI2 (3) =DT* (XZO (4) +XZI1 (4) ) 

XZI2 (4) =DT* (XZO (5) +XZI1 (5) ) 

XZI2 (5) =DT* (-K1Z (1) * (XZO (1) +XZI1 (1) ) 
x -K1Z (2) * (XZO (2) +XZI1 (2) ) 

x -K2Z (1) * (XZO (3) +XZI1 (3) ) 

x -K2Z (2) * (XZO (4) +XZI1 (4) ) 

x -K2Z (3) * (XZO (5) +XZI1 (5) ) -K3Z*A2N (3) ) 

DO 1=1,5 

XZ (I) =XZO (I) +0.5* (XZI1 (I) +XZI2 (I) ) 

END DO 
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c 

c 


4TH ORDER YAW STATE SPACE FILTER 


DO 1=1,3 

KIR (I) =R2 IR* 

x (BVR (1) *PR (1, I) +BVR ( 2 ) *PR (2,1) +BVR(3) *PR (3 , I) +PR (4,1) +DQCR (I) ) 
END DO 
K2R=R2 IR* 

x (BVR ( 1 ) *PR (1,4) +BVR ( 2 ) *PR(2,4) +BVR(3) *PR(3,4) +PR(4,4) ) 

K3R=R2 IR* 

x (BVR (1) *PR (1, 5) +BVR ( 2 ) *PR(2, 5) +BVR(3) *PR (3 , 5) +PR (4,5) -DQDR) 

DO 1=1,3 

XRI1 (I) =DT* 

x ( (AVR (I , 1) -BVR (I) *K1R (1) ) *XRO (1) 
x + (AVR (I , 2) -BVR (I) *K1R (2) ) *XRO (2) 
x + (AVR (I, 3) -BVR (I) *K1R ( 3 ) ) *XRO(3) 
x -BVR (I) *K2R*XRO (4) 

x -BVR(I) * (1+K3R) *BADNO(3) ) 

END DO 

XRI 1 (4) =DT* (-K1R (1) *XRO (1) -KIR (2) *XRO (2) 
x -KIR (3) *XRO ( 3 ) -K2R*XRO (4 ) -K3R*BADNO (3 ) ) 

DO 1=1,3 

XRI 2 (I) =DT* 

x ( (AVR (I, 1) -BVR (I) *K1R (1) ) * (XRO (1) +XRI1 (1) ) 
x + (AVR (I , 2) -BVR (I) *K1R (2) ) * (XRO (2) +XRI1 (2) ) 
x + (AVR (I , 3) -BVR (I) *K1R (3) ) * (XRO (3) +XRI1 (3) ) 
x -BVR (I) *K2R* (XRO (4) +XRI1 (4) ) 

x -BVR(I) * (1+K3R) *BADN(3) ) 

END DO 

XRI2 (4) =DT* (-K1R (1) * (XRO (1) +XRI1 (1) ) 
x -KIR (2) * (XRO (2) +XRI1 (2) ) 

x -KIR (3) * (XRO (3) +XRI1 (3) ) 

x -K2R* (XRO (4) +XRI1 (4) ) -K3R*BADN (3) ) 

DO 1=1,4 

XR (I) =XRO (I) +0.5* (XRI 1 (I) +XRI2 (I) ) 

END DO 
C 

c * ************ UPDATE ALL THE DUMMY VARIABLES: ******************** 

C 

DO 1=1,9 

XXO (I) =XX (I) 

XYO ( I ) =XY ( I ) 

END DO 
C 

DO 1=1,5 

XZO (I) =XZ (I) 

END DO 

DO 1=1,4 

XRO ( I ) =XR ( I ) 

END DO 

A2NO ( 1 ) = A2N ( 1 ) 

A2NO ( 2 ) = A2N ( 2 ) 

A2NO ( 3 ) = A2N ( 3 ) 


105 



BADNO ( 1 ) = BADN ( 1 ) 
BADNO ( 2 ) = BADN ( 2 ) 
BADNO ( 3 ) = BADN ( 3 ) 

RETURN 

END 
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8.17. vmult.f 


c 

C Subroutine VMULT : MATRIX MULTIPLICATION. 

C 

SUBROUTINE VMULT ( A, B , C , K, L , M) 

DIMENSION A (K, L) , B (K, L) ,C(K,M) 

DO 20 KK = 1 , K 

DO 20 MM = 1 , M 

C ( KK , MM ) = 0.0 

DO 20 LL = 1 , L 

C ( KK , MM ) = C ( KK , MM ) + A (KK, LL) *B (LL , MM) 

20 CONTINUE 
RETURN 
END 
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8.18. washopt3.f 


C***** SUBROUTINE WASHOPT3 . FOR ****** 

C 

C OPTIMAL WASHOUT ALGORITHM: ANG . VELOCITY DEVELOPMENT. 

C GIVEN A/C ACCELS ACA AND EULER RATES BETAAD , 

C COMPUTE SIMULATOR INERTIAL DISPLACEMENT AND EULER ANGLES. 

C 

SUBROUTINE WASHOPT3 (MODE) 

INCLUDE 'comint2.com' 

INCLUDE 'wcom2.com' 

INCLUDE 'optint3.com' 

INCLUDE 'wopt3.com' 

INCLUDE 'matrixlc.com' 

C 

C Compute Fairing Parameters 

C 

SQWASH=SQWASHP*EA+SQWASHI * (1 . O-EA) 

DELSQ=MAX ( MIN ( SQWASH - SQWASHP ,1.0) ,-1.0) 
SQWASHP=SQWASH+DELSQ 

A=1 . 0- SQWASHP 
AA=1 . O-SQWASHI 
C 

C Fairing of Heave Nonlinear Gain and Limit 

C 

GZ3 (1) =AA*GZ3 0 (1) +SQWASHI *GZ3 S (1) 

GZ3 (2) =AA*GZ3 0 (2) +SQWASHI *GZ3 S (2) 

GZ3 (3) =AA*GZ30 (3 ) +SQWASHI *GZ3 S (3) 

AMX3 =AA* AMX3 0 + SQWASHI *AMX3 S 
C 

C Fairing of Runway Roughness Amplitude 

C 

XKA=A*XKAO+ SQWASHP *XKAS 

IF (MODE . EQ . 1 ) THEN 
H = DT 
C 

C Set "old" variables for future use in 0FIL3 

C 

DO 1=1,3 
A2N0 (I) =0 . 

BADNO (I) =0 . 

END DO 

DO 1=1,8 

X10 (I) =0 . 

X20 (I) =0 . 

X30 (I) =0 . 

X40 (I) =0 . 
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END DO 


DO 1=1,6 

X50 (I) =0 . 

X60 (I) =0 . 

END DO 

CALL RESETC2 
C 

C Set "old" variables for future use in HOLD and OPERATE modes 

C 

ASIO (1) =XDD 
ASIO (2) =YDD 
ASIO (3) =ZDD 
VSIO (1) =XD 
VSIO (2) =YD 
VSIO (3) =ZD 
SSIO (1) =x 
SSIO (2) =Y 
SSIO (3) =Z 
BETASRO (1) =PHI 
BETASRO (2) =THE 
BETASRO (3) =PSI 
BSDRO (1) =PHID 
BSDRO (2) =THED 
BSDRO (3) =PSID 

DO 1=1,2 

XHO (I) =0 . 

ACAO (I) =0 . 

BSDTO (I) =0 . 

BETASTO (I) =0 . 

BETASTLO (I) =0 . 

XTO=0 . 

XT2O=0 . 

WGUSTO=0 . 

ACZT=0 . 

END DO 

GO TO 1 
END IF 

IF (MODE. EQ. 2) CALL WTRIM3 
C 

C Compute Augmented Acceleration from W-Gust 

C 

IF (MODE. EQ. 3) THEN 

WGAV=0 . 5* (WGUST+WGUSTO) 

XT=XTO+DT* ( -G1D1*XT0+XT20+ (G1N1-G1D1*G1N2) *WGAV) 

XTAV=0 .5* (XT+XTO) 

XT2=XT20+DT* (-G1D0*XTAV+ (G1N0 -G1D0 *G1N2 ) *WGAV) 
ACZT=XT+G1N2 * WGUST 
XTO=XT 
XT20=XT2 
WGUS TO = WGUST 
C 

C (first-order turbulence model no longer used) 


109 



c 

C XT=XTO+DT* ( -G1D0 *XTO+ (G1NO -G1DO *G1N1 ) *WGUSTO) 

C ACZT=XT+G1N1*WGUST0 

C XTO=XT 

C WGUSTO=WGUST 

END IF 

CALL LI BA 
CALL GAINOPT3 

A2N ( 3 ) =A2N ( 3 ) +GT2 *ACZT 

CALL OFIL3 

IF (MODE . EQ . 3 ) CALL INTEG3 

1 RETURN 
END 
C 
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8.19. winit2.f 


SUBROUTINE WINIT2 
C 

C THIS ROUTINE LOADS THE INITIAL VALUES INTO THE WASHOUT 

C PARAMETER ARRAYS. 

INCLUDE ' comint2 . com ' 

INCLUDE 'wcom2.com' 

INCLUDE 'optint3.com' 

INCLUDE 'wopt3.com' 

INCLUDE 'matrixlc.com' 

REAL DERIV ( 64 ) 

C 

EQUIVALENCE (DERIV (1) , THESDD ) 

C 

C INITIALIZATION OF ADAPTIVE ALGORITHM DERIVATIVES 

C 

DO 10 I = 1,64 
DERIV (I) = 0. 

10 CONTINUE 

C 

Qk k k k k k k k k k k k k k k k k k k k k k k k k k k k k kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk 

C NOTE: RX , RY , RZ HAVE DIFFERENT MEANING FROM THOSE IN UTIAS REPORT. 

C R IS THE VECTOR FROM C.G. TO CENTROID OF A/C . 

C RX = SSI IN ( 1 ) 

C RY = SSIIN (2 ) 

C RZ = SSIIN (3 ) 

DATA RX / 0 . / , RY / 0 . / , RZ/O . / 

kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk 

c 

C FOR 737 MOTION BASE RX,RY,RZ = 12.192,0.2286,1.7399 

C 

C INTEGRATION CONSTANTS 

C 

C*******changed by wu************ 

DATA T / 0 . / 

DATA INT /O/ 

DATA NEQ /30/ 

q * ************** CHANGED BY WU ********************* 

C** GAINS ARE CHANGED FOR CONVENIENCE OF COMPARISON 
C** BETWEEN NASA & ADAPTIVE ALGORITHMS. 


c 

DATA XI 

/ 3 . 6576/ 

c 

DATA SXO 

/O . 500/ 

c 

DATA Y1 

/2 .4384/ 

c 

DATA SYO 

/ 0 . 5 0 / 

c 

DATA Z1 

/I .2192/ 

c 

DATA SZO 

/O . 50/ 


DATA XI 

/ 3 . 6576/ 
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DATA SXO 

/i.o/ 


DATA Y1 

/2 .4384/ 


DATA SYO 

/i.o/ 


DATA Z1 

/I .2192/ 


DATA SZO 

/i.o/ 

C* *************************************************** 


DATA PI 

/ • 18/ 


DATA SPO 

/i.o/ 


DATA Q1 

/ - 5/ 


DATA SQO 

/i.o/ 


DATA R1 

/ - 15/ 


DATA SRO 

/i.o/ 

C 

c 

LONGITUDINAL PARAMETERS 

c 


DATA WX 

/0 . 00929/ 


DATA BX 

/ 0 . 01 / 


DATA CX 

/ .2/ 


DATA DX 

/ . 707/ 


DATA EX 

/ • 25/ 


DATA GAMX 

/ . 1640419948/ 


DATA KLX 

/ .3229173125/ 


DATA KDX 

/ . 0107639104/ 


DATA KILX 

/ -02/ 


DATA KIDX 

/ - 5/ 


DATA LAMXL 

/-.!/ 


DATA LAMXU 

/ 1 ■ / 


DATA DELXL 

/o . / 


DATA DELXU 

/ 1 ■ / 


DATA LAMXDL 

/- -06/ 


DATA DELXDL 

/-1000 . / 


DATA LAMXO 

/i.o/ 


DATA DELXO 

/0 . 5/ 

c 

c 

DATA WX 

/10 . 0/ 

c 

DATA BX 

/ 8 0 . 0/ 

c 

DATA CX 

/50 . 0/ 

c 

DATA DX 

/4 . 5/ 

c 

DATA EX 

/ 2 . 25/ 

c 

DATA GAMX 

/ • 12/ 

c 

DATA KLX 

/3 .5/ 

c 

DATA KDX 

/0 . 10/ 

c 

DATA KILX 

/ -02/ 

c 

DATA KIDX 

/ - 5/ 

c 

DATA LAMXL 

/-■I/ 

c 

DATA LAMXU 

/!-/ 

c 

DATA DELXL 

/o ./ 

c 

DATA DELXU 

/!-/ 

c 

DATA LAMXDL 

/- • 06/ 

c 

DATA DELXDL 

/-1000 . / 

c 

DATA LAMXO 

/I.O/ 

c 

DATA DELXO 

/I.O/ 


c 

C LATERAL PARAMETERS 

C 

DATA WY /O . 00929/ 
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DATA 

BY 

/0.1/ 

DATA 

CY 

/2 . 0/ 

DATA 

DY 

/I .2727/ 

DATA 

EY 

/0.81/ 

C* ********** CHANGE BY WU *********** 

C DATA GAMY 

/O . 0328084/ 

DATA 

GAMY 

/ . 1640419948/ 

C* *********************************** 

DATA 

KLY 

/O . 516668/ 

DATA 

KDY 

/O .269098/ 

DATA 

KILY 

/ - 05/ 

DATA 

KIDY 

/l. 5/ 

DATA 

LAMYL 

/-■I/ 

DATA 

LAMYU 

/ - 8/ 

DATA 

DELYL 

/o . / 

DATA 

DELYU 

/ - 3/ 

DATA 

LAMYDL 

/- • 06/ 

DATA 

DELYDL 

/- • 2/ 

DATA 

LAMYO 

CO 

o 

DATA 

DELYO 

/0 . 3/ 


c 


c 

DATA WY 

/ 1500 . / 


c 

DATA BY 

/4 0 0 . 0/ 


c 

DATA CY 

/100 . 0/ 


c 

DATA DY 

/ 12 . 80/ 


c 

DATA EY 

/10 . 24/ 


c 

DATA GAMY 

/0 . 05/ 


c 

DATA KLY 

/0 . 085/ 


c 

DATA KDY 

/0 . 15/ 


c 

DATA KILY 

/ - 05/ 


c 

DATA KIDY 

/l. 5/ 


c 

DATA LAMYL 

/-■I/ 


c 

DATA LAMYU 

/ • 2/ 


c 

DATA DELYL 

/o ./ 


c 

DATA DELYU 

7-3/ 


c 

DATA LAMYDL 

O 

1 


c 

DATA DELYDL 

/ - . 04 / 


c 

DATA LAMYO 

/ 0 . 2 / 


c 

DATA DELYO 

/ 0 . 3 / 


c 




c 

VERTICAL PARAMETERS 


c 





DATA BZ 

/ • 1/ 



DATA CZ 

/0.1/ 



DATA DZ 

/I . 2727/ 



DATA EZ 

/ • 8 1/ 



DATA KEZ 

/ . 516669/ 



DATA KIEZ 

/ - 05/ 



DATA ETAZL 

/o . / 



DATA ETAZU 

7-25/ 



DATA ETAZDL 

kO 

o 

1 



DATA ETAZO 

7-25/ 


c 




c 

YAW PARAMETERS 


c 





DATA BPS 

/1.0/ 



DATA EPS 

/0 . 3/ 
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c 

c 

c 


c 

c 

c 


DATA 

KEPS 

/100 . 0/ 

DATA 

KIEPS 

/0.1/ 

DATA 

ETAPSU 

/i.o/ 

DATA 

ETAPSL 

/O . / 

DATA 

ETAPSDL 

/ - • 4 / 

DATA 

ETAPSO 

/ 1 - / 

TOUCHDOWN PARAMETERS 

DATA KLXS 

/ 3 . 2291731: 

DATA 

KILXS 

/ - 1/ 

DATA 

LAMXDLS 

/-•!/ 

DATA 

LAMXOS 

/ 1 - / 

DATA 

WYS 

/ . 009290304/ 

DATA 

BYS 

/ -01/ 

DATA 

CYS 

/ • 2/ 

DATA 

DYS 

/ . 707/ 

DATA 

EYS 

/ • 25/ 

DATA 

GAMYS 

/ . 1640419948/ 

DATA 

KLYS 

/ 3 . 229173125/ 

DATA 

KILYS 

/ - 1/ 

DATA 

L AMYLS 

/-•!/ 

DATA 

LAM YUS 

/ 1 - / 

DATA 

LAMYDLS 

/-•!/ 

DATA 

L AMY OS 

/i.o/ 

DATA 

ETAZUS 

/ - 5/ 

DATA 

ETAZOS 

/ - 5/ 


FLIGHT PARAMETERS 

KLXO = KLX 
KILXO = KILX 
LAMXDLO = LAMXDL 
LAMXOO = LAMXO 


C 

C 

C 

C 

C 

C 


WYO 

= 

WY 

BYO 

= 

BY 

CYO 

= 

CY 

DYO 

= 

DY 

EYO 

= 

EY 

GAMYO 

= 

GAMY 

KLYO 

= 

KLY 

KILYO 

= 

KILY 

LAMYLO 

= 

LAMYL 

LAMYUO 

= 

LAMYU 

LAMYDLO 

= 

LAMYDL 

L AMY 00 

= 

LAMYO 

ETAZUO 

= 

ETAZU 

ETAZOO 

= 

ETAZO 


Runway Roughness Parameters 

DATA XKAO/O . /,XKAS/0 . 0 0 02 7/ , WB/3 0 . 0 
Fairing Parameters 
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DATA SQWASHI / 0 . 0/, EA/O . 93 94 13 / , SQWASHP/O . 0/ 

C 

C COMPENSATION TERMS 

C 

DATA CCXA / . 0 0 6 9/ 

DATA CCYA /.0069/ 

DATA CCZA / . 0 0 6 9/ 

DATA CCXV / . 15/ 

DATA CCYV / . 15/ 

DATA CCZV / . 13 3 / 

DATA CCPS / . 12 / 

DATA CCTH / . 12 / 

DATA CCPH / . 12 / 

C 

C GRAVITATIONAL CONSTANT 

C 

DATA G / 9 . 806178/ 

C 

C Initialization of Optimal Algorithm Inputs 

C 

DATA SSIl/3*0.0/,VSl/3*0. 0 / , BETASl/3 * 0 . 0/ 

C 

C Initialization of Dummy Variables 

C 

DATA A2NO / 3 * 0 . / , BADNO / 3 * 0 . / 

DATA XIO/ 8*0 . /,X2O/8*0 . /,X3O/8*0 . /,X4O/8*0 . / 

DATA X50/ 6*0 . /,X60/ 6*0 . / 

DATA SSIO/ 3*0 . 0/, VSIO/3*0 . 0/, ASIO/3*0 . / 

DATA BSDTO/ 2*0.0/, BSDRO/ 3*0.0/ 

DATA BETASRO/ 3*0 . 0 / , BETASTO/ 2*0 . 0 / , BETASTLO/2 * 0 . 0/ 

DATA XHO/ 2*0.0/, ACAO/ 2*0.0/ 

DATA T1N1, T1D0, T2N1, T2D0/-0 . 019,0.333333,0.019,0.333333/ 
C 

C Tilt Coordination Limit 

C 

DATA BDLIM/0 . 0873/ 

C 

C Nonlinear Scaling Coefficients 

C 

DATA GX/0 . 6, -0 . 046, 0 . 0014/ 

DATA GY/0.6,-0.064,0.0026/ 

DATA GZ0/0 . 6, -0 . 082, 0 . 0038/ 

DATA GZS/0.6, -0.01, 0.0/ 

DATA GP/ 1.0, -1.35, 0.6/ 

DATA GQ/ 1.0, -0 . 5, 0 . 0/ 

DATA GR/ 1.1, -1.46, 0.64/ 

C 

C Translational and Rotational Limits 

C 

DATA AMX0/10 . /,BMX/1 . /, AMXS/50 . / 

C 

C Parameters for optimal roll/sway channel filters. 

C 

DATA R1N6 , R1N5 , R1N4 , 

+ R1N3 , R1N2 , R1N1 , R1N0 , 

+ R2N6 , R2N5 , R2N4 , 

+ R2N3 , R2N2 , R2N1 , R2N0 , 
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c 

c 

c 


c 

c 

c 


+ R4N6 , R4N5 , R4N4 , 

+ R4N3 , R4N2 , R4N1 , R4N0 , 

+ R1D5 , R1D4 , 

+ R1D3,R1D2,R1D1,R1D0/ 


+ 

1.0006, 

15 . 9733 , 

42 . 0572, 


+ 

56 . 8102, 

43 . 5211, 

16 . 2283 , 

1.2344, 

+ 

-0 . 0008, 

-1 . 1663 , 

-4 . 7183 , 


+ 

-3 . 5599, 

-1.2765, 

-0 . 0785, 

0 . 0000, 

+ 

3 .4188, 

24 .4014, 

29 . 7868, 


+ 

-0 . 0048, 

0 . 0000, 

0 . 0000, 

0 . 0000, 

+ 

+ 

16.3952, 
58 . 6718, 

43 . 7546, 
44 . 7900, 

16 . 7041, 

1 .2927/ 


Parameters for optimal pitch/surge channel filters. 

DATA P1N6 , P1N5 , P1N4 , 

+ P1N3 , P1N2 , P1N1 , PINO , 

+ P2N6 , P2N5 , P2N4 , 

+ P2N3 , P2N2 , P2N1 , P2N0 , 

+ P4N6 , P4N5 , P4N4 , 

+ P4N3 , P4N2 , P4N1 , P4N0 , 

+ P1D5 , P1D4 , 

+ P1D3 , P1D2 , P1D1 , P1D0/ 


+ 

1.0006, 

15 . 9733 , 

42 . 0572, 


+ 

56 . 8102, 

43 . 5210, 

16.2282, 

1.2344, 

+ 

0 . 0008, 

1 . 1663 , 

4 . 7183 , 


+ 

3 . 5599, 

1 .2765, 

0 . 0785, 

-0 . 0000, 

+ 

3 .4188, 

24 .4014, 

29 . 7868, 


+ 

-0 . 0048, 

0 . 0000, 

0 . 0000, 

0 . 0000, 

+ 

+ 

16.3952, 
58 . 6718, 

43 . 7546, 
44 . 7900, 

16 . 7041, 

1 .2927/ 


Parameters for optimal yaw channel filters. 

DATA Y1N4 , Y1N3 , Y1N2 , Y1N1 , Y1N0 , 

+ Y1D3 , Y1D2 , Y1D1 , Y1D0/ 


+ 0.9734, 11.7588, 1.6345, 0.0000, 0.0000, 

+ 17.1168, 10.1205, 1.4510, 0.0232/ 

C 

C Parameters for optimal heave channel filters. 

C 

DATA H1N4 , H1N3 , H1N2 , H1N1 , H1N0 , 

+ H1D3 , H1D2 , H1D1 , H1D0/ 

+ 0.2779, 0.0540, 0.0000, 0.0000, 0.0000, 

+ 1.5321, 1.1124, 0.2736, 0.0209/ 


C 

C Nonlinear Scaling Coefficients 

C 

DATA GX3/0 . 6, -0 . 055, 0 . 002/ 

DATA GY3/0 . 5, -0 . 055, 0 . 002/ 

DATA GZ30/0 . 6, -0 . 082, 0 . 0038/ 
DATA GZ3S/1 . 3 , -0 . 0375, 0 . 0003/ 
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DATA GP3 /O. 3,-0. 3,0.1/ 

DATA GQ3/0 .4, -0 . 54, 0 . 26/ 

DATA GR3/1 . 1, -1 .46, 0 . 64/ 

C 

C Translational and Rotational Limits 

C 

DATA AMX30/10 . / , BMX3 / 1 . / , AMX3 S/2 0 . / 

C 

C Augmented Turbulence Parameters 

C 

DATA G1D0 , G1D1 , G1N0 , G1N1 , G1N2 
+ /25. 0,12. 5, 2. 5, 12. 0,14. 4/ 

DATA XTO , XT20/ 2*0 . 0/, WGUSTO/0 . 0/,ACZT/0 . 0/ 
DATA GT2/0 . 8/ 

RETURN 

END 
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8.20. winit4.f 


SUBROUTINE WINIT4 
C 

C THIS ROUTINE LOADS THE INITIAL VALUES INTO THE WASHOUT 

C PARAMETER ARRAYS. 

INCLUDE 'nopt4.com' 

REAL BRBXVEC (66) , R1PXVEC (66) , ZXVEC (66) 

REAL BRBYVEC (66) , R1PYVEC (66) , ZYVEC (66) 

REAL BRBRVEC (15) , R1PRVEC (15) , ZRVEC (15) 

REAL BRBZVEC (21) ,R1PZVEC(21) ,ZZVEC(21) 


C 

C 

C 

C 

C 

C 


Initialization of Nonlinear Algorithm Inputs 
DATA XXO/ 9*0 . /XYO/9*0 . /,XRO/4*0 . /,XZO/5*0 . / 

Parameters for nonlinear roll/sway channel filters. 

DATA ALPY /0.0/, ALPYMAX /l.O/, Q2Y /0.0,0.8/ ( MUY /4.0E-6 / 


DATA APY / 

+ -0.48601433, 
+ 0.93783312, 

+ -0.22785440, 
+ 0.92719057, 

+ 0.00000000, 
+ -0.00000000, 
+ 0.17852148, 

+ 0.74839400, 

+ 0.84518815, 

+ 0.08172733, 

+ 0.47047268, 

+ -1.63405743, 
+ 0.00000000, 
+ 0.00000000, 
+ 0.00000000, 
+ 0.00000000, 
+ 0.00000000, 
+ 0.00000000, 
+ -0.00007038, 
+ 0.00036336, 

+ -1.33333333, 
+ 1.33333333, 

DATA BRBYVEC 
+ 1.77789635, 

+ -1.77838998, 
+ 1.78953524, 

+ -1.77168162, 
+ 0.00064319, 

+ 0.00142583, 

+ 0.05316003, 

+ -0.27311620, 
+ 0.05897769, 


1.50095561, 0.43295640, 

0.0000, 0.0000, 0.0000, 

1.23627334, 0.43548518, 

0.0000, 0.0000, 0.0000, 
0.00000000, 0.00000000, 
0.0000, 0.0000, 0.0000, 
-0.42864297, 0.00091112, 

0.0000, 0.0000, 0.0000, 

0.23802370, 0.00091112, 

0.0000, 0.0000, 0.0000, 
-2.17119788, -0.42654860, 
0.0000, 0.0000, 0.0000, 
0.00000000, 0.00000000, 
0.0000, 0.0000, 0.0000, 
0.00000000, 0.00000000, 
1.0000, 0.0000, 0.0000, 
0.00000000, 0.00000000, 
0.0000, 1.0000, 0.0000, 
0.00070080, 0.00016391, 

0.0000, 0.0000, 0.0000, 
-1.33333333, 0.00000000, 

0.0000, 0.0000, 0.0000, 


-2.00166583, 2.30852675 

0.0000, 0.0000, 

-2.00719798, 2.32816934 

0.0000, 0.0000, 
-0.50000000, 0.50000000 

0.0000, 0.0000, 
-0.01882892, 0.19024153 

0.0000, 0.0000, 

0.18117108, -0.00975847 

0.0000, 0.0000, 
1.84882203, -2.39757925 

0.0000, 0.0000, 
0 . 00000000 , 0.00000000 
0.0000, 0.0000, 
0 . 00000000 , 0.00000000 
0.0000, 0.0000, 
0 . 00000000 , 0.00000000 
0.0000, 0.0000, 
-0.00074143, 0.00089035 

-1.000000, 0.0000, 
- 0 . 20000000 , 0.20000000 
0.0000, -3.14159265/ 


1.77659704, -0.00027616, 0.26791584, 

0.0000, 0.0000, 1.33333333, 0.0000 

0.00274996, 0.25422767, -0.25172914, 

0.0000, 0.0000, 1.33333333, 0.0000 

-0.00290936, 0.00349374, 

0 . 0000 , 0 . 0000 , - 0 . 00000000 , 0.0000 
-0 . 05580338, 

0 . 0000 , 0 . 0000 , 0 . 20000000 , 0.0000 


-0.26816675 

0 . 0000 , 

0 . 0000 , 

0 . 0000 , 

0 . 0000 , 
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+ 

0.27441166, 

0 . 0000, 

0 . 0000 

, -0.20000000, 0.0000, 

0 . 0000, 

+ 

1 . 78093859, 

0 . 0000, 

0 . 0000 

, -1.33333333, 0.0000, 

0 . 0000, 

+ 

0.0000, 0. 

.0000, 0. 

00000000 

, 0.0000, 

0 . 0000, 


+ 

0.0000, 0. 

. 00000000, 

0 . 0000 

, 0.0000, 



+ 

1 . 00000000, 

0 . 0000, 

0 . 0000 

! 



+ 

0.0000, 0. 

. 0000, 





+ 

0 . 0000/ 






DATA R1PYVEC 

/ 





+ 

8 . 63477261, 

7.08082902, 0. 

00000000, 

-0 . 55988506, 

-0.55988506 

+ 

-12 . 57237449, 

0 . 0000, 

0 . 0000 

, 0.0000, 

-0 . 11033547, 

0 . 0000, 

+ 

5 . 80695939, 

0.00000000, -0. 

45897456, 

-0.45897456, 


+ 

-10.30873990, 

0 . 0000, 

0 . 0000 

, 0.0000, 

-0 . 11097991, 

0 . 0000, 

+ 

0 . 00000000, 

-0.00000000, -0. 

00000000, 



+ 

-0 . 00000000, 

0 . 0000, 

0 . 0000 

, 0.0000, 

0 . 00000000, 

0 . 0000, 

+ 

0 . 03635800, 

0 . 03635800, 




+ 

0 . 81558635, 

0 . 0000, 

0 . 0000 

, 0.0000, 

-0 . 00023219, 

0 . 0000, 

+ 

0 . 03635800, 






+ 

0 . 81558635, 

0 . 0000, 

0 . 0000 

, 0.0000, 

-0 . 00023219, 

0 . 0000, 

+ 

18.30829171, 

0 . 0000, 

0 . 0000 

, 0.0000, 

0 . 10870250, 

0 . 0000, 

+ 

8 . 00000000, 

0 . 0000, 

0 . 0000 

, 0.0000, 

0 . 0000, 


+ 

4 . 00000000, 

0 . 0000, 

0 . 0000 

, 0.0000, 



+ 

1 . 00000000, 

0 . 0000, 

0 . 0000 

§ 



+ 

1.0000, 0. 

. 00000000, 





+ 

0 . 0000/ 






DATA R2IY /4 . 

. 17714546E 

-005, 1 

. 00000000/ 




DATA AVY / 


+ 

-0.3001182, 

-0.3501544, 

o 

o 

1 

-0 . 0432552, 

-0 . 0432552, 

-0 . 0219541, 

+ 

-0 . 0408725, 

-0 . 6256486, 

o 

o 

1 

-0 . 0373488, 

-0 . 0373488, 

-0 . 0382025, 

+ 

0 . 0000000, 

0 . 0000000, 

o 

o 

-0.5000000, 

0.5000000, 

-0 . 0000000, 

+ 

0 . 1789127, 

-0.4325385, 

o 

o 

-0 . 0147076, 

0 . 1852924, 

0 . 7463742, 

+ 

0 . 8455793 , 

0.2341282, 

o 

o 

1 

0 . 1852924, 

-0 . 0147076, 

0 . 0797075, 

+ 

0.2873278, 

-0.3474845, 

o 

o 

-0 . 0806039, 

-0 . 0806039, 

-0 . 6884752/ 


DATA BVY / 

+ 1.684826,-16.777107, -3.924000, 17.749601,-21.314831,-8.698809, 


+ 1.333333, 1. 

333333 , 

-0 . 000000, 

0.200000, -0 

.200000, 

-1.333333/ 

DATA DQCY / 

+ 2641.408443, 

2656 . 

. 836237, 

-0 . 000000, 



+ 5.558626, 

5 . 

.558626, 

2602 .315320/ 



DATA ZYVEC / 






+ 1.0, 

-1.0, 

-1.0, 

-1.0, 

-1.0, 


+ -1.0, 

-1.0, 

-1.0, 

-1.0, 

-1.0, 

1.0, 

+ 1.0, 

-1.0, 

-1.0, 

-1.0, 



+ -1.0, 

-1.0, 

-1.0, 

-1.0, 

-1.0, 

1.0, 

+ 1.0, 

-1.0, 

-1.0, 




+ -1.0, 

-1.0, 

-1.0, 

-1.0, 

-1.0, 

1.0, 

+ 1.0, 

-1.0, 





+ -1.0, 

+ 1.0, 

-1.0, 

-1.0, 

-1.0, 

-1.0, 

1.0, 

+ -1.0, 

-1.0, 

-1.0, 

-1.0, 

-1.0, 

1.0, 

+ 1.0, 

-1.0, 

-1.0, 

-1.0, 

-1.0, 

1.0, 

+ 1.0, 

-1.0, 

-1.0, 

-1.0, 

-1.0, 


+ 1.0, 

-1.0, 

-1.0, 

-1.0, 



+ 1.0, 

-1.0, 

-1.0, 




+ 1.0, 

+ 1.0/ 

DATA PYVEC / 
+ 8.78919914, 

-1.0, 






119 



+ 

8 . 64675152, 

2 . 04454782, 

-5 

+ 

-8.38739821, 

-21 . 02141997, 

-25 

+ 

9 . 07316301, 

2 .21477279, 

-4 

+ 

-8.39486263 , 

-21 . 05959150, 

-25 

+ 

0 . 71635788, 

-1.20003026, 

0 

+ 

-1 . 74100084, 

-4.45442250, 

-5 

+ 

5.44431336, 

0 . 18951117, 

3 

+ 

4 . 13378917, 

10 . 88195613 , 

13 

+ 

4 .49770413, 

-3 . 84403097, 


+ 

-4 . 01767838, 

-10.33185268, 

-12 

+ 

8 . 54553179, 



+ 

8.27250466, 

20.46798345, 

23 

+ 

23 . 81195147, 

33 . 64457162, 

29 

+ 

71 . 85128707, 

71 . 56090170, 

-0 

+ 

82.30634518, 

-0 . 06573714, 

19 

+ 

0.49999450, 

-0 . 01797396, 


+ 

5 . 06794969/ 




18310331, 

3.53876667, 

-7 . 88399752 

10394095, 

0 . 01775090, 

-6.40258907 

18934594, 

4 . 64570745, 

-7 . 69883299 

21567898, 

0 . 01774509, 

-6.43447599 

98991751, 

-1.32889642, 


59861393 , 
77151434, 

0 . 02613645, 

-1.44219625 

90068610, 

-0 . 02161095, 

3 . 56872382 

56415459, 

0 . 02169120, 

-3 . 19600160 

69772741, 

-0 . 01783242, 

6 . 00808010 

33323432, 

-0 . 00748064, 

6 . 63816656 

03103290, 

98961531, 

16 . 77905545, 



c 

c 

c 


DO J=l, 11 

APYO (J) =APY (J, J) 

DO 1=1,11 

IF(I.GE.J) THEN 

BRBY ( I , J) =BRBYVEC ( (J-l) *11-J* (J-l) /2 + I) 

BRBY ( J, I) =BRBY ( I , J) 

R1PY (I , J) =R1PYVEC ( (J-l) *11- J* (J-l) /2+I) 

R1PY ( J, I ) =R1PY ( I , J) 

PY (I , J) =PYVEC ( (J-l) *11- J* (J-l) /2+I) 

PY ( J, I ) =PY ( I , J) 

ZY (I , J) =ZYVEC ( (J-l) *11- J* (J-l) /2+I) 

ZY(J, I) =ZY(I, J) 

END IF 
END DO 
END DO 

Parameters for nonlinear pitch/surge channel filters. 

DATA ALPX /0.0/, ALPXMAX /l.O/, Q2X /Q.0,0.6/, MUX /4.0E-6/ 


DATA APX / 

+ 0.35214308, 
+ -1.44348410, 
+ 0.60755620, 
+ -1.45137984, 
+ 0.00000000, 
+ -0.00000000, 
+ 0.17753181, 
+ 0.74938368, 
+ 0.84419847, 
+ 0.08271701, 
+ -0.37464499, 
+ 0.75422005, 
+ 0.00000000, 
+ 0.00000000, 
+ 0.00000000, 
+ 0.00000000, 
+ 0.00000000, 
+ 0.00000000, 


-1.72273247, 0.43037039, 

0.0000, 0.0000, 0.0000, 

-1.99016155, 0.42784160, 

0.0000, 0.0000, 0.0000, 
0 . 00000000 , 0 . 00000000 , 
0.0000, 0.0000, 0.0000, 
-0.42963264, -0.00091112, 

0.0000, 0.0000, 0.0000, 

0.23703402, -0.00091112, 

0.0000, 0.0000, 0.0000, 
1.04552994, -0.43677819, 

0.0000, 0.0000, 0.0000, 
0.00000000, 0.00000000, 
0.0000, 0.0000, 0.0000, 
0 . 00000000 , 0 . 00000000 , 
1.0000, 0.0000, 0.0000, 
0 . 00000000 , 0 . 00000000 , 
0.0000, 1.0000, 0.0000, 


-2.38099014, 1.90345806, 

0.0000, 0.0000, 

-2.36134755, 1.89792591, 

0.0000, 0.0000, 

-0.50000000, 0.50000000, 

0.0000, 0.0000, 
-0.00975847, 0.18117108, 

0.0000, 0.0000, 
0.19024153, -0.01882892, 
0.0000, 0.0000, 
2.29193764, -2.05630186, 

0.0000, 0.0000, 
0.00000000, 0.00000000, 
0.0000, 0.0000, 
0 . 00000000 , 0 . 00000000 , 
0.0000, 0.0000, 
0 . 00000000 , 0 . 00000000 , 
0.0000, 0.0000, 
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+ 

- 0 . 00024842 , 

0 . 00052276 , - 0 . 

00016391 , 

0 . 

00089035 , 

- 0 . 00074143 

+ 

0 . 00054140 , 

0 . 0000 , 0.0000 

, 0 . 0000 , 

-1 

. 000000 , 

0 . 0000 , 

+ 

- 1 . 33333333 , 

- 1 . 33333333 , 0 . 

00000000 , 

-0 . 

20000000 , 

0 .20000000 

+ 

1 . 33333333 , 

0 . 0000 , 0.0000 

, 0 . 0000 , 

0 

. 0000 , - 3 . 

. 14159265 / 

DATA BRBXVEC 

/ 





+ 

1 . 77925517 , 

1 . 77466884 , 0 . 

00097480 , 

0 . 

26137162 , 

- 0.26225730 

+ 

- 1 . 78099759 , 

0 . 0000 , 0.0000 

, 1.33333333 

, 0 . 0000 , 

0 . 0000 , 

+ 

1 . 78432002 , 

- 0 . 00205131 , 0 . 

27780923 , 

-0 . 

27594547 , 


+ 

- 1 . 77100221 , 

0 . 0000 , 0.0000 

, 1.33333333 

, 0 . 0000 , 

0 . 0000 , 

+ 

0 . 00064319 , 

- 0 . 00349374 , 0 . 

00290936 , 




+ 

- 0 . 00212447 , 

0 . 0000 , 0.0000 

, - 0.00000000 

, 0 . 0000 , 

0 . 0000 , 

+ 

0 . 05897769 , 

- 0 . 05580338 , 





+ 

- 0 . 25512671 , 

0 . 0000 , 0.0000 

, 0.20000000 

, 0 . 0000 , 

0 . 0000 , 

+ 

0 . 05316003 , 






+ 

0 . 25705694 , 

0 . 0000 , 0.0000 

, - 0.20000000 

, 0 . 0000 , 

0 . 0000 , 

+ 

1 . 78479499 , 

0 . 0000 , 0.0000 

, - 1.33333333 

, 0 . 0000 , 

0 . 0000 , 


+ 0 . 0000 , 0 . 0000 , 0 . 00000000 , 0 . 0000 , 0 . 0000 , 

+ 0 . 0000 , 0 . 00000000 , 0 . 0000 , 0 . 0000 , 

+ 1 . 00000000 , 0 . 0000 , 0 . 0000 , 

+ 0 . 0000 , 0 . 0000 , 

+ 0 . 0000 / 

DATA R1PXVEC / 


+ 

12 . 10587580, 

13.94623075, -0. 

00000000, 

0 . 66307892, 

0 . 66307892 

+ 

-7.44252436, 

0 . 0000, 

0 . 0000 

, 0.0000, 

0 . 10967645, 

0 . 0000, 

+ 

16 . 06665966, 

-0.00000000, 0. 

76398942, 

0 . 76398942, 


+ 

-8.57318831, 

0 . 0000, 

0 . 0000 

, 0.0000, 

0 . 10903201, 

0 . 0000, 

+ 

0 . 00000000, 

-0.00000000, -0. 

00000000, 



+ 

0 . 00000000, 

0 . 0000, 

0 . 0000 

, 0.0000, 

0 . 00000000, 

0 . 0000, 

+ 

0 . 03635800, 

0 . 03635800, 




+ 

-0.40737763, 

0 . 0000, 

0 . 0000 

, 0.0000, 

-0 . 00023219, 

0 . 0000, 

+ 

0 . 03635800, 






+ 

-0.40737763, 

0 . 0000, 

0 . 0000 

, 0.0000, 

-0 . 00023219, 

0 . 0000, 

+ 

4 . 57748825, 

0 . 0000, 

0 . 0000 

, 0.0000, 

-0 . 11130943 , 

0 . 0000, 

+ 

8 . 00000000, 

0 . 0000, 

0 . 0000 

, 0.0000, 

0 . 0000, 


+ 

4 . 00000000, 

0 . 0000, 

0 . 0000 

, 0.0000, 



+ 

1.00000000, 

0 . 0000, 

0 . 0000 

§ 



+ 

1.0000, 0. 

. 00000000, 






+ 0 . 0000 / 

DATA R2IX /4 . 17714546E-005, 1.00000000/ 

DATA AVX / 


+ 

-0 

.3001182, 

-0.3501544, 

o 

1 

o 

o 

1 

.0432552, -0.0432552, 

-0 . 

0219541, 

+ 

-0 

. 0408725, 

-0 . 6256486, 

-0.0, -0 

.0373488, -0.0373488, 

-0 . 

0382025, 

+ 

0 

. 0000000, 

0 . 0000000, 

0.0, -0 

.5000000, 0.5000000, 

-0 . 

0000000, 

+ 

0 

. 1789127, 

-0.4325385, 

0.0, -0 

.0147076, 0.1852924, 

0 . 

7463742, 

+ 

0 

. 8455793 , 

0.2341282, 

-0.0, 0 

.1852924, -0.0147076, 

0 . 

0797075, 

+ 

0 

.2873278, 

-0.3474845, 

0.0, -0 

.0806039, -0.0806039, 

-0 . 

6884752/ 

DATA BVX / 







+ 

5 

. 947141, - 

12 . 514793 , 

3 . 924000, 

-21 . 

314831,17 . 749601, 

-12 

. 961123 , 

+ 

1 

. 333333 , 

1.333333, - 

0 . 000000, 

0 . 

200000, -0.200000 

, -1 

.333333/ 

DATA DQCX / 







+ 

- 

2625.631557, -2610.203763, 

-0 

. 000000, 



+ 


5.558626, 5.558626, 

2664 

. 724680/ 



DATA ZXVEC / 







+ 


1.0, 

-1.0, 

-1.0, 


-1.0, -1.0 

§ 


+ 


-1.0, 

-1.0, 

-1.0, 


-1.0, -1.0 

! 

1.0, 

+ 


1.0, 

-1.0, 

-1.0, 


-1.0, 



+ 


-1.0, 

-1.0, 

-1.0, 


-1.0, -1.0 

§ 

1.0, 
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+ 

1.0, 

-1.0, 

-1.0, 



+ 

-i.o, 

-1.0, 

-1.0, 

-1.0, -1. 

. 0, 1.0 

+ 

1.0, 

-1.0, 




+ 

-i.o, 

-1.0, 

-1.0, 

-1.0, -1. 

. 0, 1.0 

+ 

1.0, 





+ 

-1.0, 

-i.o, 

-1.0, 

-1.0, -1. 

. 0, 1.0 

+ 

1.0, 

-1.0, 

-1.0, 

-1.0, -1. 

. 0, 1.0 

+ 

1.0, 

-1.0, 

-1.0, 

-1.0, -1. 

.0, 

+ 

1.0, 

-1.0, 

-1.0, 

-1.0, 


+ 

1.0, 

-1.0, 

-1.0, 



+ 

1.0, 

-1.0, 




+ 

1.0/ 





DATA PXVEC / 





+ 

8.30351624, 





+ 

8 . 09520238, 

1 . 68093020, 

-4 .48023672, 

3 . 57953908, 

-8 . 19270520 

+ 

-8.31577879, 

-20 . 67790997, 

-24 .24095727, 

-0 . 01780137, 

-6 . 16084104 

+ 

8.45574766, 

1 . 51070523 , 

-3.37329594, 

4 . 57329645, 

-7 . 94167444 

+ 

-8.30831437, 

-20 . 63973845, 

-24 . 12921925, 

-0 . 01780718, 

-6 . 12895412 

+ 

0 . 71635788, 

-0 . 98991751, 

1.20003026, 

-2.39658160, 


+ 

-1 . 74100084, 

-4 .45442250, 

-5 . 59861393 , 

-0 . 02613645, 

-1.44219625 

+ 

4 .49770413, 

0 . 18951117, 

4 . 17497242, 



+ 

4 . 01767838, 

10.33185268, 

12 . 56415459, 

0 . 02169120, 

3 . 19600160 

+ 

5.44431336, 

-4 . 99112805, 




+ 

-4 . 13378917, 

-10 . 88195613 , 

-13 . 90068610, 

-0 . 02161095, 

-3 .56872382 

+ 

9 . 64863006, 





+ 

8.43067234, 

21.23134650, 

25 . 64717081, 

0 . 01771985, 

6 .55535001 

+ 

23 . 81195147, 

33 . 64457162, 

29.33323432, 

0 . 00748064, 

6 . 63816656 

+ 

71 . 85128707, 

71 . 56090170, 

0 . 03103290, 

16 . 77905545, 


+ 

82.30634518, 

0 . 06573714, 

19 . 98961531, 



+ 

0.49999450, 

0 . 01797396, 




+ 

5 . 06794969/ 






DO J=l, 11 

APXO (J) =APX (J, J) 

DO 1=1,11 

IF(I.GE.J) THEN 

BRBX (I , J) =BRBXVEC ( ( J- 1) *11- J* ( J- 1) /2+I ) 

BRBX (J, I) =BRBX (I , J) 

R1PX (I , J) =R1PXVEC ( (J-l) *11- J* (J-l) /2+I) 

R1PX (J, I) =R1PX (I , J) 

PX (I, J) =PXVEC ( (J-l) *11- J* (J-l) /2+I) 

PX(J, I) =PX(I, J) 

ZX (I, J) =ZXVEC ( (J-l) *11- J* (J-l) /2+I) 

ZX ( J, I) =ZX(I, J) 

END IF 
END DO 
END DO 
C 

C Parameters for nonlinear yaw channel filters. 

C 

DATA ALPR /0.0/, ALPRMAX /l.O/, Q2R /120.0/, MUR /2.0E-6 / 
DATA APR / 

+-1 .45728460E-004, -1 . 69985373E-006, -4 . 89593935E-004, 

+ -2 . 79035083E-002, 0.0, 

+ 1 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 

+1 . 86874341E-001, 2 . 17980102E-003 , -4 . 89593935E-004, 
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+ - 2 . 79035083E-002, 0 . OOOOOOOOE+OOO , 

+ 0.0, 0.0, 0.0, 0.0, 0.0, 
+5.21851607E-003,6.08715277E-005,1.75322913E-002, 

+9 . 99220787E-001, -1.0/ 

DATA BRBRVEC / 

+ 3 . 4 94 92 524E-0 02, 4 . 0 76 6 6 53 9E- 0 04 , 1 . 174 16 6 12E - 0 0 1 , -5. 2185160 7E -003, 
+ 0.0, 

+4 . 75523782E-006 , 1 . 36960937E-003 , -6 . 08715277E-005 , 0.0, 

+3 . 94476555E-001, -1.75322913E-002, 0.0, 

+7 . 79212948E-004, 0.0, 

+ 0.0/ 

DATA R1PRVEC / 

+ 7 . 79212948E-004, 0 . 0 , 7 . 792 12 94 8E - 0 04 , 0 . 0 , - 2 . 7903 50 83E - 0 02 , 

+0 . OOOOOOOOE+OOO, 0 . 0, 0 . OOOOOOOOE+OOO, 0.0, 

+7. 792 12 94 8E -004, 0.0,-2.79035083E-002, 

+2 . 00000000E+002, 0.0, 

+9 . 99220787E-001/ 

DATA R2IR / 7 . 792 12 94 8E - 0 04 / 

DATA AVR / 

+-1.87020070E-001, -2. 18 15008 7E-003, -6. 2831853 IE-001, 

+1 . OOOOOOOOE+OOO, 0 . OOOOOOOOE+OOO, 0 . OOOOOOOOE+OOO, 

+ 0 . OOOOOOOOE + OOO, 0 . OOOOOOOOE + OOO, -6. 2831853 IE -001/ 

DATA BVR /-6 . 69716293E + 000, - 7 . 8 119245 6E - 0 02 , -2 . 25000000E + 001/ 
DATA DQCR / 35.80986220, 0.00000000, 35.80986220/ 

DATA DQDR / 1 . 2 8234 623E+0 03 / 

DATA ZRVEC / 


+ 

1 . 0 , 

- 1 . 0 , 

- 1 . 0 , 

- 1.0 

+ 

1 . 0 , 

- 1 . 0 , 

- 1 . 0 , 

- 1.0 

+ 

1 . 0 , 

- 1 . 0 , 

- 1 . 0 , 


+ 

1 . 0 , 

- 1 . 0 , 



+ 

1 . 0 / 





DATA PRVEC / 

+ 0.52254103, 0.13331932, 0.53598671, -7.30258353, -6.33348013, 

+ 5.01432503, 1.07135777, 6.89175648, 5.57001008, 

+ 0.73962239, -5.97893648, -5.25519323, 

+ 323.73106725, 186.47187109, 

+ 139.87165376/ 

DO J=l, 5 

APRO (J) =APR (J, J) 

DO 1=1,5 

IF(I.GE.J) THEN 

BRBR (I , J) =BRBRVEC ( ( J- 1) *5- J* ( J- 1) /2+I ) 

BRBR (J, I) =BRBR (I , J) 

R1PR (I , J) =R1PRVEC ( (J-l) *5- J* (J-l) /2+I) 

R1PR (J, I) =R1PR (I , J) 

PR (I, J) = PRVEC ( (J-l) *5-J* (J-l) /2+I) 

PR ( J, I ) =PR ( I , J) 

ZR ( I , J) =ZRVEC ( (J-l) *5-J* (J-l) /2+I) 

ZR(J, I) =ZR(I, J) 

END IF 
END DO 
END DO 

C 

c Parameters for nonlinear heave channel filters, 
c 
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DATA ALP Z /O.O/, ALPZMAX /0.2/, Q2Z /l. 0,2.0/, MUZ /1.0E-7 / 
DATA APZ / 


+ 

-0 . 060606, 

0.139394, 

0 . 0000, 

0 . 0000, 

0 . 0000, 

0 . 

000000, 

+ 

-0.567713 , 

-0 . 767713 , 

0 . 0000, 

0 . 0000, 

0 . 0000, 

0 . 

000000, 

+ 

0 . 000000, 

0 . 000000, 

0 . 0000, 

0 . 0000, 

0 . 0000, 

0 . 

000000, 

+ 

0 . 000000, 

0 . 000000, 

1.0000, 

0 . 0000, 

0 . 0000, 

0 . 

000000, 

+ 

0 . 000000, 

0 . 000000, 

0 . 0000, 

1.0000, 

0 . 0000, 

0 . 

000000, 

+ 

-1 . 717157, 

-2 .282843 , 

0 . 0000, 

0 . 0000, 

0 . 0000, 

-62 

. 831853/ 

DATA BRBZVEC / 







+ 

2 . 948629, 

3 . 920000, 

0 . 0000, 

0 . 0000, 

1 . 717157 

, 0 

. 0000, 

+ 

5.211371, 

0.0000, 0. 

. 0000, 

2 .282843 , 

0 . 0000, 




+ 0 . 000000 , 0 . 0000 , 0 . 0000 , 0 . 0000 , 

+ 0 . 000000 , 0 . 0000 , 0 . 0000 , 

+ 1 . 000000 , 0 . 0000 , 

+ 0 . 000000 / 

DATA R1PZVEC / 

+ 200 . 0000 , 200 . 0000 , 0 . 0000 , 0 . 0000 , 0 . 0000 , 0 . 0000 , 

+ 200 . 0000 , 0 . 0000 , 0 . 0000 , 0 . 0000 , 0 . 0000 , 

+ 40.0000, 0.0000, 0.0000, 0.0000, 

+ 400.0000, 0.0000, 0.0000, 

+ 40.0000, 0.0000, 

+ 0 . 0000 / 

DATA AVZ / -0.060606, 0.139394, -0.567713, -0.767713/ 

DATA BVZ / 1.717157, 2.282843/ 

DATA ZZVEC / 


+ 

1.0, -1.0, 

-1.0, 

-1.0, 

-1.0, -1. 

.0, 


+ 

1.0, -1.0, 

-1.0, 

-1.0, 

-1.0, 



+ 

1.0, -1.0, 

-1.0, 

-1.0, 




+ 

1.0, -1.0, 

-1.0, 





+ 

1.0, -1.0, 






+ 

1.0/ 






DATA PZVEC / 






+ 

910 . 190970, 

-315.435226, 

-1 . 160668, 

-79 . 558688, 

-844 . 169063 

+ 

-13 .404542, 

327 . 670260, 

-60.259856, 

-315 . 797740, 

-198 . 948877 

+ 

-3 . 146942, 

193 . 813146, 

269 . 544193 , 

145 . 881379, 

2 .202307 

+ 

1160 . 148818, 

888 . 175899, 

13 . 591950, 

1946.358732, 

30.388540 

+ 

0.480397/ 







DO J=l, 6 

APZO (J) =APZ (J, J) 

DO 1=1,6 

IF(I.GE.J) THEN 

BRBZ (I , J) =BRBZVEC ( ( J- 1 ) *6 - J* ( J- 1 ) /2 + I ) 
BRBZ (J, I) =BRBZ (I , J) 

R1PZ (I , J) =R1PZVEC ( ( J-l) *6- J* ( J-l) /2+I) 
R1PZ (J, I) =R1PZ (I , J) 

PZ (I, J) =PZVEC ( (J-l) *6-J* (J-l) /2+I) 

PZ (J, I) =PZ (I, J) 

ZZ (I, J) =ZZVEC ( (J-l) *6-J* (J-l) /2+I) 

ZZ (J, I) =ZZ (I, J) 

END IF 
END DO 
END DO 
C 

c Nonlinear Scaling Coefficients 

c 
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DATA GX4/0 . 5, -0 . 05, 0 . 002/ 

DATA GY4/0 .4, -0 . 035, 0 . 001/ 

DATA GZ40/0 . 6, -0 . 082, 0 . 0038/ 

DATA GZ4S/2 . 0, -0 . 05, 0.0/ 

DATA GP4 /0. 3, -0.3, 0.1/ 

DATA GQ4 /O. 3, -0.3, 0.1/ 

DATA GR4 / 1 . 1,-1. 46, 0.64/ 

C 

c Translational and Rotational Limits 

c 

DATA AMX40/10 . / , BMX4/1 . / , AMX4S/20 . / 

C 

c Augmented Turbulence Parameters 

c 

DATA G2D0 , G2D1 , G2N0 , G2N1 , G2N2 
+ /25. 0,12. 5, 2. 5, 12. 0,14. 4/ 

DATA GT4 / 1 . 2 / 

RETURN 

END 
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8.21. wtrim3.f 


SUBROUTINE WTRIM3 
C 

C THIS ROUTINE USES TILT ANGLES TO TRIM OUT ANY INITIAL 

C STEADY- STATE LONGITUDINAL OR LATERAL ACCELERATIONS 

C 

INCLUDE ' optint3 . com' 

INCLUDE 'comint2.com' 

C 

C First-Order Washout Filters to Obtain Angular Rates 

C 

XH (1) =XHO (1) +DT* (-T1D0* (XHO ( 1 ) +T1N1*ACA0 ( 2 ) ) ) 

BSDT (1) =XH (1) +T1N1*ACA (2 ) 

XH (2) =XHO (2) +DT* (-T2D0* (XHO ( 2 ) +T2N1*ACA0 ( 1 ) ) ) 

BSDT (2) =XH (2) +T2N1*ACA(1) 

C 

C Integrate Rates to Obtain Angular Displacements 

C 

DO K=1 , 2 

BETAST (K) =BETASTO (K) +DT*0 . 5* (BSDT (K) +BSDTO (K) ) 
BSDTO (K) =BSDT (K) 

BETASTO (K) =BETAST (K) 

BETASTLO (K) =BETASTO (K) 

XHO (K) =XH (K) 

ACAO (K) =ACA (K) 

END DO 
C 

C Assign Variable Names To Match Modified Algorithm 

C For Input to JACKDRVR 

C 

PHI = BETAST (1) 

THE = BETAST (2) 

PHID = BSDT ( 1 ) 

THED = BSDT (2) 

RETURN 

END 
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Appendix A. Actuator Extension Limiting 

There exists the possibility that, without restriction, the output of the cueing 
algorithm may drive the motion platform beyond its hardware limits. This raises the 
requirement that the simulation software should be able to handle the situation that the 
motion platform may need to be arrested before encountering the hardware limits. A 
braking algorithm was developed [6] to perform this task. When approaching the 
hardware limits, the braking algorithm takes over the control of the motion platform from 
the motion cueing algorithm. It is also necessary that the braking algorithm return 
control to the cueing algorithm when the cueing algorithm begins to drive the platform 
toward smaller excursions. In other words, the braking algorithm should release the 
motion platform to gradually resume normal function. 

The logic of the braking algorithm is shown in Figure A.l. The algorithm makes 
a series of decisions to determine when to arrest, or “brake” the platform and while 
“braked”, to determine when to release the brake. The first decision is based on an 

evaluation of the expression 2 b 0 a b l avail -/ 2 for each actuator at each simulation cycle, 

where / is the velocity of the actuator, l avaU is the available actuator length, i.e. the 
difference between the maximum and computed actuator extensions, a h is the 
deceleration by which the braking algorithm will slow down (and stop) the actuator, and 
b Q is a coefficient set less than or equal to 1 that is described below. When one actuator 
reaches the braking region defined by 

2b 0 a h l amil -i 2 < 0, (A.l) 

it will be decelerated by a b . At the same time, all the other actuators will be decelerated 
proportionally to their respective velocities. 
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FLAG2 =0 INDICATES THAT THE BRAKE 
HAS BEEN RELEASED. 


Figure A.l. Braking Algorithm in the JACKDRVR Subroutine. 
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The braking deceleration a h , noted by the variable ACMAX in Figure A.l, should 
be set much smaller than the maximum actuator acceleration so that the perceptual effect 
of entering the braking region and stopping the motion platform is minimal. The braking 
algorithm sets a software limit position that is slightly before the simulator hardware limit 
position. When b n = 1, the actuator will be stopped exactly at the software limit position; 
when b 0 < 1, the actuator will be stopped before the software limit position. 

From Eq. A.l, the available length l avml can be found as a function of the actuator 

velocity / . Figure A. 2 shows the available length for various combinations of the 
coefficient b„ and deceleration a h . Note that a reduction in the deceleration results in an 
increase in the available actuator length when entering the braking region. For smaller 
values of deceleration, larger values of b 0 are used to reduce the available length. 
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Figure A.2. Available Actuator Length upon Entering the Braking Region. 

In order to “release” the brake, a decision is based on a comparison between the 
actual motion platform states and the cueing algorithm output. This comparison begins 


Braking Region 
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when the platform has been completely stopped by the braking algorithm. When the 
cueing algorithm output states become smaller than the corresponding actual platform 
states, the braking algorithm will “release” the brake. 

When the brake is released, the cueing algorithm output may have large velocities 
while the simulator has small velocities at that instant. To avoid the normal cueing 
algorithm output being resumed with any excessively large acceleration, an algorithm 
that allows the simulator to gradually follow the washout algorithm output was 
developed. The algorithm expresses the actuator extension command to the motion 
platform as 

horn = L, + / 0 ){hes ~ he, ) > ( A - 2 ) 

where / com is the commanded actuator extension that drives the motion platform, l act is the 
actual extension of the actuator (limited by the braking algorithm), l des is the desired 
actuator extension generated by the cueing algorithm, and f (t) is a function of time. 

During normal operation / (7) is set equal to one. After the brake is released / (/) is set 
to zero and is gradually increased to one. 
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